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Preface 



Control design of autonomous underwater vehicles is an important area for research- 
ers in the control systems community. Control is generally difficult to achieve due 
to nonlinear dynamics, uncertain models, and the presence of disturbances that are 
hard to measure or estimate. This book presents a new approach to the modeling and 
control design of autonomous underwater vehicles. 

Kinematic and dynamic nonlinear models for autonomous underwater vehi- 
cles are discussed in this book. Controllability of autonomous underwater vehi- 
cles for different motion planning tasks is reviewed. The book combines the 
feedback control design for both kinematic and dynamic models. The feedback 
control is also achieved in the presence of uncertainties, thereby making the 
control design robust. 

This work is intended for students and researchers, as there is more development 
that is needed in this particular area. The results of this book can be extended to 
obtain advanced control strategies and design schemes for autonomous underwater 
vehicles and other similar problems falling in the area of nonlinear control. 
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Introduction 



1.1 OVERVIEW 

The work presented in this book is concerned with developing models that can be 
used for the motion planning and feedback control design of underwater vehicles. 
Underwater vehicles present difficult control system design problems due to their 
nonlinear dynamics, uncertain models, and the presence of disturbances that are 
difficult to measure or estimate. Many problems must be solved to make robotic 
underwater vehicles a reality. The dynamic control of the vehicle needs to guarantee 
stability and perform consistently. The dynamics of autonomous underwater vehi- 
cles present a difficult control system design problem that traditional linear design 
methodologies cannot accommodate easily. The dynamics are fundamentally non- 
linear in nature. Hydrodynamic coefficients often are poorly known, and a variety of 
immeasurable disturbances are present due to currents. 

In this book, a new approach of feedback control methodology is developed to the 
accurate trajectory control and point stabilization of underwater vehicles. The feedback 
control is developed in both the absence and presence of uncertainties. The methods 
can deal with nonlinear dynamics directly, and are shown to be stable in the presence 
of disturbances, thereby making the control robust. 

The first part of the book presents the study concerning the applicability of kinemat- 
ic-based control of underwater vehicles. The methods of motion planning and feedback 
control for a kinematic model of an underwater vehicle are developed. The kinematic 
model of an underwater vehicle falls into a general category of nonholonomic systems. 
The system is characterized by nonholonomic constraints on its generalized velocities. 
The motion planning problem for this system, with constraints on the velocities, is trans- 
formed into a control problem having fewer control inputs than the degrees of freedom. 
The nonlinear controllability issues for the system are also studied. For the design of 
feedback controllers, the system is transformed into chained and power forms. The 
methods of transforming the kinematic model of the system into these forms are dis- 
cussed. Differential geometric control theory and nonlinear system analysis and control 
design techniques, and the results of recent research and study in the motion planning of 
nonholonomic systems are used and presented for the support of the work. 

The kinematic model of the autonomous underwater vehicle is developed, and the 
feedback controller design for the same is presented in detail and simulation results 
are obtained. Also, a brief mathematical analysis of the concepts involved in the 
study of controllability, control design, and modeling is presented. 

The kinematic-based control addresses the motion planning for a kinematic model 
of an underwater vehicle. The kinematic model belongs to nonholonomic systems. The 
control model for such systems is drift-free, nonlinear, and underactuated, given by 

k = ft(?)vi + 8i(q)v 2 + • • ■ + 8m(q)v m ( u ) 

1 
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Here qe Mis the state vector of the system, M is the state space, and M cz R", 
where n is the dimension of the configuration space M, to which the vector q 
belongs. Vector v e R' n is the input or the control vector of dimension m . Vectors 
gi(q) 6 R" ; i = 1,2, ....77? are vector fields on M and are assumed to be smooth and 
linear time invariant. The system is called drift-free because the system state does 
not change under zero-input conditions. Also, the system is underactuated because 
the dimension of the space spanned by the control vector is less than the dimension 
of the configuration space. 

A special case of Equation 1.1 with two inputs was presented in [1], In fl] the 
motion planning tasks for a car-like robot were defined and the feedback control 
design was studied. The control was achieved using various control strategies for 
each task. The kinematic-based control of an underwater vehicle is an extension of 
the work presented in [1], The extended problem is higher dimensional with four 
inputs. In this book the controllability of the system is discussed and proved as 
related to motion planning. We present feedback control laws that give global stabili- 
zation of the vehicle about a desired trajectory and about a point. This is achieved by 
transforming the kinematic into canonical chained and power forms. The book pre- 
sents the method of converting the kinematic model into chained and power forms 
via state feedback and coordinate transformation. 

For trajectory tracking of underwater vehicles [23] proposed a stable tracking con- 
trol method based on a Lyapunov function. In [22] and [23] a Lyapunov-like function 
is used to develop a nonlinear feedback control scheme. The control achieves global 
stabilization about a desired trajectory. However, the system is not point stabilizable 
with the use of the proposed controller. In this book we will be making use of the 
full state feedback (approximate linearization) scheme for trajectory tracking. This 
scheme results in local asymptotic stabilization only. Exact nonlinear control (full 
state linearization) design is used to achieve the global stabilization. In this case static 
state feedback fails to achieve the goal. However, the dynamic state feedback is used 
to serve the purpose. Here the control design is done using the chained form system. 

The kinematic model of an underwater vehicle belongs to a class of systems that 
cannot be stabilized by a pure state feedback law [2]. The design of globally asymp- 
totic stabilizing controllers for nonholonomic systems is challenging. The design is 
difficult in a sense that no time -invariant smooth static stabilizing controller exists 
for such systems [2]. Various control schemes have been adopted for this purpose. 

One way to deal with this is to use time-varying, smooth controllers. This approach 
has been extensively studied in [3] and [4], In [3] it is shown that time-varying smooth 
control laws for driftless systems have necessarily algebraic (not exponential) con- 
vergence rates. Asymptotic stabilization for underwater vehicles using time-varying 
smooth feedback laws was achieved in [4], 

Another alternative is the use of the nonsmooth feedback controllers that can 
achieve exponential convergence. These schemes have been proposed in [5] and [6]. 
In [25] a discontinuous piecewise smooth control law was proposed and exponential 
convergence to a constant desired configuration was achieved. In [15] a nonsmooth 
time-invariant controller was proposed to achieve the exponential convergence with 
stability to a constant desired configuration. The controller was implemented using 
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the chained form. In this book, we have adopted the former approach, that is, we 
used a time-varying and smooth feedback. The control design method for stabiliza- 
tion used herein is adopted from [7]. To this end, a transformation of the kinematic 
model into power form is derived. The controller achieves global stabilization to a 
constant configuration for an underwater vehicle. 

The latter part of the book addresses the feedback control and robust control 
design for a dynamic model of an underwater vehicle. The motion of underwater 
vehicles is presented as a kinematic and a dynamic model. The three-dimensional 
vehicle motion may be described in terms of the twelve nonlinear system equations: 



M 



dv(t) 

dt 



f(v,q) + g(v,q)u(t) 



dq(t) 

dt 



h(v,q) 



( 1 . 2 ) 



The first equation is the dynamic model of the system, and the second is the 
kinematic model. The vector v(t) is the velocity vector and q(t) is the position vec- 
tor in the fixed frame coordinates. In order to design the feedback control for point 
stabilization of the dynamic model, the control methodology is nonlinear feedback 
linearization. The design technique adopted for the dynamic control is backstep- 
ping. The book also discusses the feedback control for these models in the presence 
of uncertainties where the goal is to design the controllers to minimize the effect of 
uncertainties. The control is achieved using Lyapunov redesign and robust backstep- 
piug for uncertain models. In both methods we achieve the control design for the 
kinematic model first, which is then used to achieve the overall control design of the 
dynamic model. The outline of the chapters is summarized below. 

Chapter 2 gives an introduction and general overview of the motion planning of 
autonomous vehicles. The concepts of nonholonomy, underactuated systems, and a 
kinematic model of the nonholonomic systems, and some examples are shown. Then 
the general problem of motion planning and the related issues are formulated for a 
class of the nonholonomic systems, with a review of some particular applications. 

Chapter 3 presents an overview and detailed analysis of the related motion plan- 
ning tasks of an autonomous underwater vehicle. The chapter presents in detail the 
derivation of the mathematical modeling of the system. For motion planning tasks, 
the kinematic model of the system is obtained and the issues related to nonlinear 
controllability of the system are studied in detail. Finally, for the purpose of control 
design, the system is converted into a chained form. The method of converting a 
multi-input nonholonomic system into a chained form is also discussed. 

Chapter 4 presents the control design and the simulation results obtained for 
the model of an underwater vehicle developed in Chapter 3. The feedback control 
design is developed using the kinematic model of the system. The performance of 
the controllers using various techniques of control design is obtained and evaluated 
for different motion planning tasks, such as trajectory tracking, point stabilization, 
and path following. The chapter also presents the simulation results obtained for 
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different controllers. The simulation results are used to compare and evaluate the 
performance of the various controllers for different paths following tasks. 

Chapter 5 presents the control design of the dynamic model of an underwater vehicle. 
The control design is obtained and evaluated for a motion planning task of point stabili- 
zation. The control design technique adopted here is feedback linearization using back- 
stepping for a dynamic model. The problem of control and stability is formulated and 
sufficient conditions for Lyapunov stability for the designed controllers are derived. 

Chapter 6 presents a design of robust nonlinear feedback controllers for the 
dynamic model representing underwater vehicles. We discuss the feedback control 
for these models in the presence of uncertainties where the goal is to design the 
controllers to minimize the effect of uncertainties on the performance of the system. 
The robust controllers are designed for point stabilization using Lyapunov redesign 
and robust backstepping methods. 

1.2 EXAMPLES OF UNDERWATER VEHICLE CONSTRUCTION 

This section provides a brief overview of the construction of underwater vehicles. 
Readers are encouraged to study [33] and [34] for more details. 

Underwater vehicles have two main issues to deal with. They need to be able to 
stay underwater and to maneuver. The vehicles can be designed so that they stay 
underwater, so that their body weight is water neutral, or they can use actuation to 
actively stay underwater. They also need actuation to be able to go underwater from 
the water surface, move around underwater, and also come back up to the surface. 

Submarines can be dynamic or static. Dynamic submarines usually will float on 
water and are actively pushed down by propellers to keep them underwater. Static sub- 
marines, on the other hand, add water into a submarine chamber (called a ballast tank) 
to add weight to it, so the submarine can get heavier. When it wants to come up, it uses 
compressed gas to push the water out, so that the submarine becomes light again. 

Remote-controlled (RC) submarines, which can be purchased in many stores, have 
a motor that drives a propeller at the back that pushes the vehicle forward, and rudder 
and stern (and sail) planes to enable the submarines to produce yaw motion and pitch 
motion. Rudders enable the yaw motion by their rotation, and stern planes produce 
the pitch motion just like elevators do in RC planes. These actuators are shown in 
Figure 1.1. A more detailed view of the rudder and stern is shown in Figure 1.2. 

The weight distribution keeps the submarine facing right side up by keeping the 
heavier weight at the bottom, not the top. The sail plane with the stern plane keeps the 
submarine balanced. The electronics is kept inside the water-tight container (WTC) 
and contains two servos and a DC motor. One servo controls the rudder angle and 
the other controls the stern. The location of the WTC and the weight distribution are 
shown in Figure 1.3. 

The WTC generally has three compartments. The first compartment will have the 
battery and speed controller, the second will have a ballast system, and the third the 
servos. Typically, the ballast system will have a tank with pressurized air and valves. 
The valves are used to allow water to flood in, to increase the weight of the submarine, 
therefore enabling it to go down, or to let water come out when the pressurized air is let 
into the chamber, so that the weight of the submarine is reduced, to enable it to rise. The 
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FIGURE 





FIGURE 1.2 Submarine rudder and stern. 




FIGURE 1 .3 WTC location and weight distribution. 
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FIGURE 1.4 Twin-propeller-based dynamic RC submarine. 

other electronic components that are usually present are the speed controller connected 
to the radio signal receiver, which receives the signal from the remote control to con- 
trol the motor and servos; the pitch control using an accelerometer to control the pitch 
angle; and a microsafe, a device that turns the ballast system off if the radio signal is 
lost for some time, so that the submarine will float on top in case of a lost radio signal. 

Submarines communicate underwater with sonar signals using audio frequencies. 
Submarines also use GPS for navigation, but that signal is lost underwater; therefore, 
an underwater inertial navigation system (INS) is used that is composed of acceler- 
ometers and gyroscopes to measure how the submarine is moving so that it can keep 
track of its position and orientation. 

Dynamic submarines don’t have the ballast system, and therefore they have to 
be actively forced down in the water; otherwise, they float up on the surface. Many 
inexpensive dynamic RC submarines are built using twin motors that are used to 
dive, surface, turn, etc. The angle of the propellers can be changed before putting the 
submarine into water to get different movements. One such submarine is illustrated 
in Figure 1.4. 

1.2.1 Propeller Principle 

Since underwater vehicles have propellers, this section will provide a brief descrip- 
tion of the theory of propeller action. Propeller thrust is produced due to the forces 
that are created by the “wings” that are part of the propellers. We present the fun- 
damentals of how force is produced in the wings, and then how propellers produce 
thrust because of them. 

1 .2.1.1 Wings 

To understand how the air flowing around a wing section produces lift, let us study 
Figure 1.5. The figure shows fluid flowing around a wing section. The streamline (a line 




FIGURE 1.5 Airflow around a wing section (Kutta condition). 
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FIGURE 1.6 Noncirculatory airflow around a wing section. 



whose tangent shows the direction of flow at every point) on the left that ends with an 
arrow on the wing surface is the one that reaches the stagnation point on the wing. 
This means that above that the fluid flows on top of the wing, and below that point 
the fluid flows under the wing. The figure shows that the fluid from the top and bot- 
tom will have the same magnitude and direction at the trailing edge. This is a condi- 
tion (Kutta condition [33]) that creates lift from fluid flowing around a wing at some 
angle of attack. If we place a wing in a fluid flow, we can expect the streamlines to 
be as shown in Figure 1.6. This fluid flow, however, has no circulation. Without fluid 
circulation, there can be no lift. In fact, the lift on a wing is proportional to the air 
circulation about the wing (Kutta-Joukowski theorem [34]). 

According to the Kutta-Joukowski theorem, the lift per unit length of a wing is 
given by 



Lift = airspeed x air circulation x air density 



The fluid flow that follows the Kutta condition (the trailing edge boundary condi- 
tion) can be obtained by superimposing a circulatory airflow on the noncirculatory 
fluid flow. This is shown in Figure 1.7. 

There is another theorem (Kelvin’s circulation theorem [33]) that says that the 
rate of change of circulation around a closed curve that consists of the same fluid 
material is zero. When a vehicle such as an airplane starts accelerating, it does not 
follow the Kutta condition, and consequently, it does not have circulation. After the 
speed increases further, the Kutta condition is satisfied (creating circulation around 
the wings), and to make the total circulation zero, another circulating air is created 
behind the wing (called the wake). This is shown in Figure 1.8. 

Due to the addition of clockwise circulation that is flowing in the direction of the 
regular airflow, the speed on top of the wing is higher than the fluid speed below the 
wing. Bernoulli’s theorem shows that where the airspeed is high, there the pressure 
is low, and where the airspeed is low, there the air pressure is high [34]. Since the air 




FIGURE 1 .7 Total fluid flow around a wing section. 
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«C=> 







FIGURE 1 .8 Total fluid circulation around a wing section. 

pressure is higher at the bottom of the wing than at the top, there is an upward force 
(lift) produced on the wing. This is shown in Figure 1.9. 

Now when we look at a wing with a finite span, such as on an underwater vehicle 
for stabilization, because of low pressure on top of the wing, the fluid from the bot- 
tom starts circulating to the top, and the whole circulation travels behind the plane. 
This is shown in Figure 1.10. 

The lift force that is generated by the wings is related to the angle of attack (i.e., 
the angle that the fluid velocity at a distance in front of the wing makes with the wing 
chord line). When the angle of attack is at zero degrees, there is no lift produced. 
As the angle of attack is increased, the lift produced increases, up to some angle of 
attack, after which further increase in the angle decreases the lift. This is shown in 
Figure 1.11. 

1.2.1. 2 Propellers 

Propellers are extremely important for robotic underwater vehicles. They provide 
the thrust for forward motion as well as for lift. Figure 1.12 shows a three-blade pro- 
peller. Propellers can come in many blade configurations, such as two blade, three 
blade, and four blade. 

Propellers produce forward thrust by accelerating fluid back. The amount of fluid 
forced back depends on the speed of rotation of the propeller as well as at its pitch 
angle. With a small pitch angle, the amount of fluid pushed back is small, and there- 
fore less forward thrust is produced. However, in that case the propeller can rotate 
faster. With a high pitch angle, the blades rotate slower but produce more thrust by 
pushing more fluid back. Propellers can be designed to have fixed pitch, or can be 
made such that the pitch of the propeller can be changed in real time (by an actuator). 
The concept of variable pitch can be compared to having gears in a motor. The pitch 
could be controlled automatically (using some electronics) to produce a constant 
speed of rotation. 



Low pressure 




High pressure 



FIGURE 1.9 Lift due to pressure difference produced on a wing section. 
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Starting vortex 




FIGURE 1.10 Vortex. 



Zero angle 
of attack 



No lift 




■ Lift produced 



FIGURE 1.11 Angle of attack and lift. 




FIGURE 1.12 Propeller. 
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Blade face 




FIGURE 1.13 Propeller parts. 



Another way to get a good estimate of the propeller thrust is to apply the theory of 
the wing section to the propeller blade to figure out the lift force that is generated on the 
blade as the fluid flows across it. The fluid speed that any part of the propeller blade is 
subjected to depends on the distance of that part from the center of rotation. Therefore, 
the propeller blades are twisted since at different radii, the relative speeds are different, 
and that makes the angle of attack different, if the blade is not designed to be twisted. 

Let us look at the parts of a propeller in more detail (see Figure 1.13). The leading 
edge of the propeller is where the fluid first cuts through the blade. The trailing edge 
is where the fluid exits the propeller surface. The blade face is the side of the blade 
that pushes the fluid away, creating a positive pressure on the surface. The blade back 
is the back side of the blade, which gets a negative pressure as the fluid is moved 
away from that region by the rotating blade. 

As the propeller rotates, the blade face pushes the fluid back and the back side 
starts pulling the fluid in (push-pull mechanism), similar to a fan, and this creates a 
forward thrust from the propeller (see Figure 1.14). 

As the propeller makes one revolution, the geometric pitch is the distance the 
propeller would have moved through a soft material like softwood if it were a 
screw. The effective pitch (smaller than the geometric pitch) is the actual distance 




FIGURE 1.14 Propeller thrust. 
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Geometric pitch 




FIGURE 1.15 Propeller pitch. 



the plane or a boat (or any other robot) moves. This difference is due to the propeller 
slippage through the fluid (see Figure 1.15). 

1.2.2 Commercially Available Underwater Vehicles 

Autonomous underwater vehicles (AUVs) have been designed in laboratories and 
research centers since around the 1950s, but there are only a few companies that 
sell these commercially. Chief among these are Kongsberg Maritime (http://www. 
km.kongsberg.com) and Bluefin Robotics Corporation (http://www.bluehnrobotics.com). 
Below is a brief description of the AUV models available from these manufacturers. 

Kongsberg manufactures the HUGIN AUVs and REMUS AUVs, besides pro- 
viding various instruments and software for navigation. The HUGIN AUV can be 
used for subsea sensing. These AUVs are designed to go as deep as 4,500 meters 
(HUGIN 4500), and can propel and navigate themselves. They also have the abil- 
ity to handle disturbances. The REMUS AUVs have the advantage that they can be 
custom designed. These AUVs at present are available in three models: REMUS 100, 
REMUS 600, and REMUS 6000. REMUS 100 is lightweight and can go 100 meters 
deep. REMUS 600 has the ability to go to 3,000 meters deep, while REMUS 6000 
has a maximum operation depth of 6,000 meters. 

Bluehn Robotics has been associated with MIT’s Autonomous Underwater Vehicle 
Laboratory. The different models available are Bluefin-9, Bluefin-12, Bluefin-21, and 
Bluefin-21 BPAUV. These AUVs are designed for low as well as deep water applica- 
tions. The numbers in each of the model names refer to the AUV diameter in inches. 
These AUVs can carry varying payloads and therefore can vary in length. 

Other manufacturers of AUVs include International Submarine Engineering Ltd. 
(http://www.ise.bc.ca/) and GAVIA Autonomous Underwater Vehicles (http://www. 
gavia.is). 

Readers can check the latest models and specifications from the individual manu- 
facturers’ Web sites. 
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z 




FIGURE 1.16 World frame. 

1.3 VEHICLE KINEMATICS FUNDAMENTALS 

There are essentially six degrees of freedom that the vehicle has: three that indicate 
the position, and the other three that indicate the orientation. These six degrees of 
freedom identify the state of the vehicle. We consider the state of the vehicle with 
respect to some general frame of reference for the world (Figure 1.16). 

Although there are six degrees of freedom for the vehicle to be placed with respect 
to position and orientation, there are typically only four variables that can be directly 
controlled. These four variables can be considered more naturally in the body moving 
frame (Cartan) of reference. In the frame of reference that is attached to the body of 
the vehicle, as shown in Figure 1.16, the vehicle can move forward in the direction 
of the body x axis, and can also produce rotation about the body x, y, and z axes. We 
consider the control variables to be the velocities in these four body variables, that is, 
the linear velocity in the direction of the body x axis and the three angular veloci- 
ties. We need a way to relate these control variables to keep track of the state of the 
vehicle. We present the Frenet-Serret equations to develop this relationship. Note 
that, in practice, the actual actuation is obtained through controlling the actuators 
that produce those control velocities through the dynamics of the system. We assume 
that there are local feedback loops already developed that can control the linear and 
angular body velocities, and the aim of the controller is to control the motion of the 
vehicle in the global frame. 

1.3.1 Frenet-Serret Equations for Cartan Moving Frame 

Let’s consider a point fixed on the vehicle. We can assume the point to be the center 
of gravity for the vehicle body, or any convenient fixed point on the vehicle body. The 
position of this point is a function of time that maps time to a point in R 2 . 
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Definition: A regular parametric representation of the position vector in terms of the 
parameter s is a function x(s) e C 1 (-a,a) such that ^ ^ 0 , Vs e (—a, a). 

Definition: A real valued function t(s) e C 1 (-a, a) such that ^ 0, V.v e (-a, a) is 
called an allowable change of parameter. 

Regular parametric representations divide curves into equivalent classes, since an 
allowable change of a parameter’s relationship is reflexive, symmetric, and transitive, 
that is, it is an equivalence relationship. 

Definition: A regular curve is an equivalence class of regular parametric represen- 
tations. 

Given a curve parameterized by arc length 

x(s) : (-a, a) -> R 2 



that represents the motion of the vehicle, its unit tangent vector is given by 

t(s) = x(s) 

The plane normal to the curve at point x is called the normal plane, as shown in 
Figure 1.17. 

It is easy to verify that IlfOvjll = 1. This implies that the inner product of t(s) with 
itself is 1, that is, 



<t(s), t(s ) > = 1 

Differentiating this equation with respect to the arc length variable s, we get 

< i(s),t(s) > + < ) > = 0 




FIGURE 1.17 Normal plane. 
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Since the inner product in real vector space is commutative, we get 

< i(s),t(s ) >= 0 



which implies that t(s) _L t(s). The curvature of the curve, K (s), is the norm of the tan- 
gent vector. Let the unit vector in the direction of i(s ) be called n(s). Hence, we get 

i(s) = K(s)n(s) 

The plane containing the vectors t(s) and n(.s) is called the osculating plane, as shown 
in Figure 1.18. 

We have that ll«(^)ll = 1. This implies that the inner product of n(s) with itself is 
1, that is. 



<n(s), n(s) > = 1 

Differentiating this equation with respect to the arc length variable s, we get 
< h(s),n(s) > + < n(s),h(s) > = 0 

Since the inner product in real vector space is commutative, we get 

< h(s),n(s ) > = 0 



which implies that h(s) ± n(s). 

We define the binormal to the curve as 

b(s) = t(s) x n(s) 
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which also gives us 

n(s ) = b{s) x t(s ) 

Differentiating the equation gives us 

b(s ) = i(s ) x n(s) + t(s ) x h(s) 

This gives, after substitution, 

b(s) = k(j) (n(s) x n(s)) + t(s ) x h(s) 

Since the cross product of a vector with itself is zero, this gives 

i>{s ) = t(s ) x h(s) 

The plane containing the vectors t(s) and b(s ) is called the rectifying plane, as shown 
in Figure 1.19. 

Since h is perpendicular to n, we have 

h(s) = (J.(i)f(i') + T (s)b(s) 

This gives us 

i>(s ) = t(s) x [|a,(i , )f(i) + x(i)fc(5)] 

Again, since a cross product of a vector with itself is zero, we get 
b{s) = X b(s)\ = -x(s)n(s) 



b(s) 




FIGURE 1.19 Normal, osculating, and rectifying planes. 
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Since n(s) = b(s) x t(s), we get by differentiating 

h(s) = b(s) xt(s)+ b(s ) x i(s ) 

which after substitutions gives 

ii(s) = — T(s)[n(,s) x t fr)] + K(i)[/7(i) x n(s)\ 

= K(s)t(s) + T(s)b(s) 

Hence, in summary we have 

x(s) — t(s) 
i(s) = K (s)n(s) 
h(s) = K(s)t(s) + T (s)b(s) 
b(s) = -x(s)n(s) 

The vectors t(s), n(s), and h(s) are all orthogonal to each other; therefore, the matrix 
containing them as the column vectors belongs to a matrix group SO(3). 



Thus, 



[t(s),n(s)Ms)\ e 50(3). 



t(s) 

0 



n(s ) b(s) 

0 0 



x(s) 

1 



e SE( 3) 



which gives us 



t(s) 


nis) 


b(s ) 






t(s) 


n(s) 


b(s) 


x(s) 


0 


0 


0 


1 




0 


0 


0 


1 



0 -k(j) 0 1 

K(s) 0 T(s) 0 

0 — t(.s’) 0 0 

0 0 0 0 
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The above equations are called the Frenet-Serret equations of a curve. The evo- 
lution of this curve in R 2 is given by 



8 = gx 



where g e SE(3) group and X is an element of the Lie algebra se( 3). We can regard 
the curvature k(.y) and the torsion 't(.v) as inputs to the system, so that if 

ufs) = K(,y) 
u 2 (s) = — T(.y) 

then 

0 -«! 0 1 

Mj 0 — u 2 0 

8 ~ 8 n n n 

0 u 2 0 0 

0 0 0 0 

which is the special case of the general form describing the state evolution of a left 
invariant control system in SE( 3). 

An example of the general form of a left invariant control system in SE(3) is given 
by an aircraft flying in R 2 . 



0 



-u 2 

0 



— w 3 
0 
M] 
0 



li 2 W4 

— M, 0 

0 0 

0 0 



The inputs rq, u 2 , and u 3 control the roll, pitch, and yaw of the aircraft, and the input 
m 4 controls the velocity in the forward direction. 

The special case of above in SE(2) is an example of a unicycle rolling on the plane. 



0 —u 2 



8 = 8 



u 2 



0 



0 

0 



1 

0 

0 



In this case, the input u 2 controls the angle of the wheel. 

The previous formulation describes kinematic steering problems, as it is assumed 
that we have direct control of the velocities of the rigid bodies. However, in physical 
systems, we only have control of the forces and torques that drive the motion of the 
body. The more realistic approach then is to formulate the steering problem using a 
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dynamic model of the system, where the forces and torques are the inputs. The con- 
trol of the dynamic models is much harder to achieve than the kinematic models. In 
the rest of this book the focus will be to address the control design of both the models 
for an autonomous underwater vehicle. 



1.3.2 Mathematical Background for Rigid Motion in a Plane 

We notice that for an underwater vehicle that has a propeller to thrust it forward and 
has three more actuators to control the three rotations, roll, pitch, and yaw, we can 
use Frenet-Serret equations to represent the kinematic equations for the motion. We 
will study these kinematics and rigid motion of the vehicle in more detail in this sec- 
tion. First, we start with rotations. 

We will first study three interpretations of a rotation matrix that we will derive in 
this section, followed by the general rigid motion in a plane. Rotations and transla- 
tions constitute rigid motions since they do not change the length of vectors that they 
transform. However, we do not want to allow reflections as rigid motions. 

1 .3.2.1 Rotation of a Vector 

We will derive a rotation matrix that is an operator that works on vectors and rotates 
them without changing the length. 

Figure 1.20 shows a rotation of angle 0 of the x axis counterclockwise. Hence, the 
vector (1,0)' has been transformed into (cos0, sin0)'. This can be written as 



cos0 

sin0 



= R d 



1 

0 



Similarly, for the y axis, when rotated by the same angle, we get 



- sin 0 

COS0 



= R e 



0 

1 




FIGURE 1.20 Rotation in a plane of the x axis. 
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FIGURE 1.21 Rotation in a plane of the y axis. 

This is shown in Figure 1.21. 

Combining these two results, we get the matrix representation for the linear rota- 
tion operator that rotates the vector counterclockwise by angle 0: 



R e = 



cos 8 
sin 9 



-sin0 

COS0 



1.3. 2. 2 Vector Represented in a Rotated Frame 

This interpretation of the rotation matrix is explained using Figure 1.22. Given a 
vector V, its representation in R 2 is transformed to its representation in the frame F 2 , 
by using the rotation matrix as follows: 



v F l = RqV*- 



Flere, the vector v F > is the representation of the vector in R 2 in frame Fp the vec- 
tor v Fl is the representation of the vector in R 2 in frame F 2 . It is clear that the angle 
between the vector and the x axis of the frame gets rotated by angle 0. 

1.3. 2. 3 Representation of a Rotated Frame 

In this interpretation the rotation matrix represents the axis of one frame of reference 
with respect to another. Refer to Figure 1.23. 




FIGURE 1 .22 Rotation of the frame for the vector. 
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FIGURE 1.23 Rotation of the frame. 



The vector representation of the x axis of the frame F 2 is 









cos 8 
sin 8 



And the vector representation of the y axis of the frame F 2 is 



ypi - 



-sin 8 
cos 8 



Combining the two we get the rotation matrix. If the unit vectors in the frame F x are 
given by vectors t, and /,, and those of the frame F 2 are given by i 2 and j 2 . then the 
rotation matrix, as shown here, is 



R e = 



h-h 

h-.ii 



h-h 

h-h 



1.3. 2.4 Group Representation 

Since rotations are additive, we can represent them as a group. For the definition of 
a group refer to the next section. We can use an exponentiation notation for the rota- 
tion, as given by 






cos 8 
sin 8 



-sin 8 
cos 8 



Here, the vector z in the power indicates that the rotation is about that axis. Another 
nice motivating example for using exponential representation is as follows. First, we 
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review the vector cross product. In order to do that, we start using vectors in R 3 . Given 
two vectors, their cross product is shown below. 





a x 




b\ 


a = 


a 2 


= a^i + a 2 j + a 3 k , b = 


b 2 




a 3 




b 3 



b t i + b 2 j + b 3 k 



where 



1 




0 




0 


0 


, j = 


1 


, k = 


0 


0 




0 




1 



then 



a x b= ( a 2 b 3 — a 3 b 2 )i + {a 3 b t — a x b 3 )j + (a^ — a 1 b l )k 



This operation can be represented conveniently in a determinant notation as follows: 



ax b = 



J 

a 2 



b, b 2 



k 

a 3 

b 3 



However, more importantly, the cross product by a fixed vector can be given a matrix 
operator form as follows. According to this, a cross product by a vector is equivalent 
to a matrix operation. 





1 

CN 

Q 

m 

G 

1 

O 

i 




b l 


ax b = [a x]b = 


a 3 0 -a, 




b 2 




1 

o 




b 3 



The matrix [a x] is skew symmetric since [axf+ [a x] = 0 , where T denotes transpose 
and zero is the matrix with all zero-valued terms. 

Consider the position vector of a particle rotating in a circle with angular velocity 
CO, as shown in Figure 1.24; then the velocity vector is given by 

p{t) = co x p(t) = [co x]p(t) 
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FIGURE 1 .24 Rotation of the position vector. 

This is a linear differential equation, whose solution is 

p(t) = e l<s,x] ' p( 0) 



For this problem, we know that the position is given in terms of the rotation matrix. 
Hence, we have 

pit) = e l<ax]t p{ 0) = e® m) p( 0) 



An infinitesimal rotation can be obtained by taking a very small angle increment in 
rotation. So, if we take the rotation angle to be very small, then we get sin0 = 0 and 
cos0 = 1, and therefore the rotation is 



So, an incremental change from zero rotation gives the tangent space of skew- 
symmetric matrices by taking 0 and finding the incremental rotation difference as 

[^co(m-Az) — R(OI 1/A/ — [ R(. ); R (ll \t — R m ]IAt = R a ,[R aAl -I]/At = R w 



0 -co 

co 0 



1.3. 2. 5 Homogeneous Representation 

To represent affine transformation, that is, rotation and translation for a two- 
dimensional motion for a vehicle, we can use homogeneous representation. Observe 
the translation and rotation of a frame in Figure 1.25. 

Any vector that is rotated and then translated is transformed as follows: 



V 






^ ab 


v 0 


1 




0 


1 


1 
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F 2 




FIGURE 1.25 Rotation and translation. 



This shows that the vector v 0 is transformed to v as 

v = R e v 0 + t ah 

We can develop three-dimensional versions of these transformations, which allow 
for arbitrary rotations and translations for the vehicle. Details of these are provided 
in [35], 



1.4 LIE GROUPS AND LIE ALGEBRAS 

This section covers the mathematical preliminaries that are used for the develop- 
ment, analysis, and control of the kinematic model of an underwater vehicle. The 
definitions and the mathematical framework presented here will be used throughout 
the book. 



1.4.1 Matrix Groups 

Definition (Group): A group G is a set with a binary operation (.): G x G -A G, such 
that V a, b, c in G, and the following properties are satisfied: 

1. Closure: a e G,b e G => a ■ b e G 

2. Associativity: (a.b).c = ci.(b.c) 

3. Identity: 3 an identity e 3 a.e = e.a = a 

4. Inverse: 3 an inverse tr 1 3 a. a 1 = a~ l . a - e 

A group is called abelian if a.b = b.a, V a,b e G 

Definition (Homomorphism): A homomorphism between groups 0: G — 3 H is a 
map that preserves the group operation: 

0(a.b) = 0(a). 0(b) 
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Definition (Isomorphism): An isomorphism is a homomorphism that is bijective. 

Definition (Field): A field K is a set with two binary operations, addition (+) and 
multiplication (.), such that: 

1. K is an abelian group under (+), with identity 0. 

2. A'-(O) is an abelian group (.), with identity 1. 

3. (.) distributes over the (+) 3 a.(b + c) = a.b + a.c 

Definition (Algebra): An algebra is a vector space with a multiplication that dis- 
tributes over addition. M n (k) is an algebra with multiplication defined as the usual 
multiplication for matrices. For A, B,C e M n (K) 

A(B + C) = AB + BC 

(B + C)A = BA + CA 

Definition (Unit): If A is an algebra, x e A is a unit if there exists y e A such that 
xy = yx = 1 . 

If A is an algebra with an associative multiplication operation and U e A is the set 
of units in A, then U is a group with respect to this multiplication operation. 

Definition (Matrix group): The class of groups whose elements are n x n matrices 
is called a matrix group. 

Definition (General and special linear groups): 

1. The group of units of M n (K ) is the set of matrices M for which det(M) V 0, 
where 0 is the additive identity of K. This group is called the general linear 
group and is denoted by GL(n,K ). 

2. SL(n,k) e GL(n,k) is the subgroup of GL(n.k) whose elements have determi- 
nant 1. This group, SL(n,k), is called the special linear group. 

Definition (Orthogonal matrix groups): 0{n,K) e GL{n,k) is the subgroup of 
GL(n,k ) whose element matrices A satisfy the orthogonality condition A r = A -1 , 
where A T is the complex conjugate transpose of A. Oin) = 0(n,R) is called the 
orthogonal group and Uin) = U(n,C) is called the unitary group. 

Definition (Special orthogonal matrix groups): 

1. SO(n) = 0(n ) n SL(n,R ) is the set of all orthogonal matrices of determinant 1. 

It is called the special orthogonal group. 

2. SU(n) = U(n) n SL(n,C ) is the set of all unitary matrices of determinant 1. 

It is called the special unitary group. 
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Definition (Euclidean matrix groups): 

1. The Euclidean group is the set of matrices E(n) such that 



E(ri) 



|Aei? ( " +1|x( ” +l1 : ,4 = 



R 

Qlxn 



P 

1 



R e GL(n),p e R n 



2. The special Euclidean group is the set of matrices SE(n) such that 



SE(n) 



|a e R(n+l)x(n+\) . A 



R p 

0 lxn 1 



R e SO(n), p e R" 



Definition (Dimension of a matrix group): The dimension of a matrix group G is 
the dimension of the vector space T of tangent vectors to G at I. 

Now let us introduce a family of matrices that will be used to determine the 
dimensions of matrix groups. Let so(n) denote the set of all skew-symmetric matri- 
ces in M n (R ): 



so(n) = { A e M n {R ): A T + A = 0} 



Similarly, the set 



su(n) = {Ae M n (C ) : A T + A = 0} 



denotes the skew-hermitian matrices, and the set 

spin) = {Ae M n (H) :A T + A = 0} 



denotes the skew-symplectic matrices. We also define 



sl(n) = [A e M n (HR): trace(A) = 0} 



and 



se(n') 



| A £ R(n+\ )'/.{ n+l ) • ^ — 



w 

0 



P 

0 



w e SO(n),p e R n 
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1.4.2 Lie Groups 

The discussion of matrix Lie groups needs some definitions from the differential 
geometry. The material in this section is treated in the context of dynamical control 
systems in [36] and [37] and from a topological point of view in [38]. 

Definition (Topological space): A topological space is a set M together with a col- 
lection of subsets T of M, that satisfies the following: 

1. T contains 0 and M. 

2. The union of any collection of sets in T is in T . 

3. The intersection of a finite collection of sets in T is in 71 

The topological space is denoted by ( M,7 ), where T is called a topology on M. 

Definition (Homeomorphism): Two topological spaces, M and N, are said to be 
homeomorphic if there exists a function / : M — > N that is continuous and bijective 
(i.e., one to one and onto) and whose inverse, / ! is continuous. The function / is 
called a homeomorphism. 

Definition (Manifold): An u-manifold is a topological space M such that for any 
p e M, there exists a neighborhood of p that is homeomorphic to an open set in R". 
The dimension of M is n. 

Definition (Smooth manifold): A smooth manifold is a manifold that has an associ- 
ated maximal atlas. 

We now introduce the concept of tangent vectors to manifolds. Let M be a differen- 
tiable n -manifold with p e M. Let 

A(p) = {iyvjy. p e W,W open in M,f: W — > R is smooth ] 

Definition (Tangent vector): A tangent vector at p e M is a linear map c, : A(p) — > R 
that satisfies the following, all f g e A(p): 

1. If/= g in a neighborhood of p, then £(/) = ^(g). 

2. Product rule: %(fg) =f(p)c,(g) + %{f)g(p). 

The tangent space to p at M, denoted by T p M, is the set of all tangent vectors to M 
at p. 

Definition (Vector field): A vector field on n-dimensional manifold M at p is a map- 
ping X : T p M —> N. A smooth vector field on a manifold is an assignment of X p e T p 
M for each p e M , such that if/ : M — > R is a smooth function, then 



(XJ)=X p (f):M^R 



is smooth over p. 
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Definition (Integral curve): Given a vector field/on M, a smooth curve c:(t l ,t 2 ) M 
is called an integral curve if 

c(t) = f(c(t)), for all t e (t ] ,t 2 ) 

An integral curve on M is a curve that follows a given vector field at each point. 
Vector fields thus represent differential equations on manifolds. The space of smooth 
vector fields becomes an algebra under the appropriate multiplication operation. 

Definition (Lie algebra): A vector space V together with the binary operation [•, •]: 
V x V —> V is called a Lie algebra if [■, •] satisfies the following for all A, B, C e V 
and r, k e R: 

1. Antisymmetry: [A,B] = — [B,A], 

2. Bilinearity: [rA + kB, C] = r[A,C] + k[B,C]. 

3. r[A,B] = [rA,B] = [ A,rB ]. 

4. Jacobi identity: [A,[B,C]] + [B,[A,C]] + [C,[A,B]] = 0. 

The derivative of a smooth function X in the direction of the vector field/is given by 

L f X{p) = (f(p))(X) 

and is the definition of a Lie derivative. 

Given two vector fields, /and g, a new vector field can be defined by 

(imp)) x = (L f L^)(p) - (L g L f X)(p) 

and | fg] is called the Lie bracket of/ and g. A vector space with the Lie bracket as the 
binary operation forms a Lie algebra. Denote by V"(M ) the space of C vector fields 
on M. 

Together with the Lie bracket operation, V°(M ) is an algebra. 

Definition (Distribution): A distribution on M is a mapping A: M — > TM that assigns 
to each p e M a subspace of the tangent space 7/W. Furthermore, A is called a 
smooth distribution if for every pe M there exists a neighborhood U and a collection 
of smooth vector fields/, i = 1 w, such that for all q e U 

A(q) = spam(fj(q)\ i = 



The dimension of A (q) is m. 

A distribution A is called involutive if, given any two vector fields/g e A, the Lie 
bracket (fg) is also in A. 

A distribution A defined on an open set U is called nonsingular if there exists 
some integer in such that for all q e TJ 

dim (f/A (q)) = m 
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Definition (Lie group): A lie group is a group G that is also a differential manifold 
such that for x, y e G, 

1. (x,y) i— > xy 

2. X i-> x~‘ 

are smooth functions. 

All finite dimensional Lie groups may be represented as matrix groups. The 
matrix groups GL(n,R), 0(n), SO(n), E(n), and SE(n) are all Lie groups. 

Let G be a Lie group with identity /, and let X f be a tangent vector to G at I. For 
any g e G, define the left translation by g to be a map L : G — > G such that L Ax) = 
gx, where x e G. Since G is a Lie group, L is a diffeomorphism of G for each g. 
Diffeomorphism is a smooth inveritable function that maps one differential manifold 
to another. Taking the differential of L g at e results in a map from the tangent space of 
G at e to the tangent space of G at g: 

dL-Jfi -> Tfi 



such that 



X g = dL g ( Xg ) 

The vector field formed by assigning x e T g (G ) for each g e G is called a left invari- 
ant field. The left invariant vector fields of G form an algebra under [•, •], which is 
called the Lie algebra of G and denoted by L(G). L(G ) is actually a subalgebra of the 
Lie algebra of all smooth vector fields on G. 

The mathematical framework presented here will be utilized for developing the 
vehicle kinematics and for studying controllability of the kinematic model of autono- 
mous underwater vehicles. The vehicle kinematics are obtained using rotations and 
translations, which are represented as Lie groups in the general framework of mani- 
folds. However, the control design is done in terms of local coordinates. These local 
coordinates are the parameterizations of the global coordinates, and therefore have 
singularities, which are also present in the control design. 
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This chapter gives an overview of the motion planning and issues related with the 
various motion planning tasks of the autonomous underwater vehicles. The control 
of the kinematic model obtained for such vehicles involves the concepts of non- 
holonomy. It will be seen that the vehicles are nonlinear and underactuated in nature 
because of the nonholonomic constraints on their generalized velocities. Finally, the 
motion planning problem will be formulated for autonomous underwater vehicles, 
and issues related with the various motion planning tasks and the feedback control 
design for these examples will be discussed. 



2.1 MOTION PLANNING OF NONHOLONOMIC SYSTEMS 

The initial motivation for the work presented here comes from the research work 
done in order to do the motion planning and control design for the nonholonomi- 
cally constrained systems. Motion planning for nonholonomic systems has been 
studied in great detail, and a lot of research is being done in this field. This problem 
has attracted researchers because of its challenging theoretical nature and practical 
importance. The nonholonomic constraints arise in a number of advanced robotic 
systems, and the application of such systems is numerous. The problem is also inter- 
esting because its theoretical behavior presents a number of challenges. First, such 
systems are underactuated, that is, the number of control inputs is less than the num- 
ber of the states or the variables of the system to be controlled. Thus, motion plan- 
ning implies that the systems can be completely controlled with a fewer number 
of actuators, thereby improving the overall cost-effectiveness of the system. Also, 
underactuation can provide backup control techniques for a fully actuated system. 
Second, both planning and control are more difficult than for holonomic systems. 
Some of the motion control problems that have been studied in detail are those of 
regulation (stabilization) and tracking. 

The problem of stabilizing such systems is a big issue, as it has been proved by 
Brockett [2] that the nonholonomic control systems with restricted mobility cannot 
be stabilized to a desired configuration (equilibrium) using a smooth time-invariant 
state feedback law. Because of this fact, there has been extensive study of this prob- 
lem. Some authors have proposed nonsmooth or discontinuous control laws. Others 
have proposed smooth but time -varying control laws for the purpose of regulation, 
and some have proposed the combination of both, that is, discontinuous time -varying 
control laws [8, 9], The method of transforming the kinematic model into the chained- 
form model and doing the control on the same was first proposed by [10] for the case 
of a car-like robot. The study of feedback control of a nonholonomic car-like robot 
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is done in [1], Various motion planning tasks, such as tracking a time-varying refer- 
ence trajectory, path following, and point-to-point stabilization of a car-like robot, 
were presented in [1], The work presented in Chapter 3 is along the same lines as 1 1], 
extended and modified for the application of underwater vehicles. The design of 
feedback controllers will be used for different motion tasks utilizing the kinematic 
model of the system. The kinematic model will be developed using the definition of 
nonholonomic constraints. 



2.2 NONHOLONOMIC CONSTRAINTS 

System constraints on the mechanical systems whose expression involves generalized 
coordinates and velocities are known as kinematic constraints of the system, which 
are also known as the nonholonomic constraints. These are of the following form 

a i (q,q) = 0, i = 1,2, ... ,k <n (2.1) 

where q is the generalized coordinate vector or the state vector, qe M cz R n , where n 
is the dimension of the configuration space M, to which the vector q belongs. These 
will limit the admissible motions of the system by restricting the set of generalized 
velocities that can be attained at a given configuration. Usually such constraints are 
in mechanics in the Pfafhan form: 



aj{q)q = 0, i = \,2,...,k<n 



(2.2) 



or 



C(q)q = 0 (2.3) 

This means they are linear in the generalized velocities. a t (q) e M cz R" ,i = 1.2, ... ,U 
are row vectors. The vectors a t : M R" are assumed to be smooth and linearly 
independent. The matrix C(q) e R" xn is a constraint matrix. 

The kinematic constraints restrict the motion by limiting the set of generalized 
velocities. The nonholonomic constraints cannot be integrated to the positions. Thus, 
while the instantaneous mobility that a system can perform is restricted to (n - 1)- 
dimensional null space of the constraint matrix C(q). we can still say that it is pos- 
sible that any configuration in the state space M can be reached. In general, for a 
system with n coordinates and k nonholonomic constraints, although the velocities 
are restricted to (n - k)-dimensional space, the global controllability in the configu- 
ration space is still attainable. 

The kinematic model of an underwater vehicle or any such system is developed 
without taking into consideration the actual dynamic forces acting on it. Such systems 
have kinematic constraints on their velocities. These constraints mostly arise due to 
the rolling of two surfaces against one another and a roll without the slip condition. 
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as in case of a wheel and the road. These can also arise due to the conservation laws 
applicable to the system or from the nature of the control inputs physically applied 
to the system [11]. Thus, nonholonomic constraints allow the global movement of the 
system in the configuration space, while at the same time restricting or reducing the 
degrees of freedom or motion performed locally by the system. 

The concept of nonholonomy is related to controllability of the corresponding 
control system. Redefining the constraint specification as the directions or degrees of 
freedom in which the system can move, rather than the direction in which it cannot 
move, is equivalent to stating the controllability problem of the corresponding con- 
trol system. Thus, we can safely say that if the system is maximally nonholonomic, 
the system is controllable, as any point in the configuration space can be reached. 
This way a motion problem can be converted into a control problem. 

Nonholonomic constraints arise in a number of ways and in various mechanical 
systems and applications. These can arise because of the reasons already given in 
the previous paragraph. For a more detailed analysis, the reader is referred to [11] 
and [12]. Some of the typical examples of the nonholonomic systems can be sum- 
marized as 

• Wheeled mobile robots 

• Space robots 

• Underwater vehicles 

• Satellites 

• Multifingered hand manipulators 

• Hopping robots 

2.3 PROBLEM DESCRIPTION 

The motion planning tasks for nonholonomic systems are attained through the use 
of the feedback controllers. The basic motion tasks considered for an underwater 
vehicle are as follows: 

Point-to-point motion: Here a desired goal configuration must be reached by 
a system starting from a given initial configuration. 

Path following: Here the system has to reach a desired final configuration 
starting from a given initial configuration, while at the same time it has to 
follow a given geometric path. The initial configuration can be considered 
to be either on or off the path. 

Trajectory following: Here the system must reach a final configuration while 
following a trajectory in the Cartesian space (i.e., a geometric path with an 
associated timing law) starting from a given initial configuration (either on 
or off the trajectory). 

The tasks are assumed here such that the systems work in an obstacle-free environ- 
ment and are shown in Figure 2.1a to c for a car-like robot. 
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FIGURE 2.1 Motion planning tasks for a car-like robot. 

The tasks can be obtained using either the feed-forward (open-loop) or feedback 
(closed-loop) control, or a combination of both. Since the feedback control is gener- 
ally robust and can work well in the presence of disturbances, use of it is preferred. 

Thinking in terms of controls, a point-to-point task can be thought of as a regula- 
tion control problem or a posture stabilization problem for an equilibrium point in 
the state space. The trajectory following is a tracking problem such that the error 
between the reference and the desired trajectories asymptotically goes to zero. 

For nonholonomic systems, the tracking or path following, or both, is easier than 
the stabilization, whereas usually the reverse is true. This difference can be explained 
by drawing a comparison between the numbers of inputs and outputs (or states) to be 
controlled. In case of a regulation problem, m inputs (two in case of a car-like robot) 
are required to regulate or control n independent control variables or states (four in 
the case of a car-like robot), with m less than n. Thus, point-to-point stabilization is 
the most difficult of the three. In case of path following and trajectory tracking, the 
output to be controlled has a dimension (p) equal to that of the input ( m ). Thus, these 
control problems are square and their difficulty level is similar to and less than the 
stabilization one. For a car-like robot, in the case of path following m is 1 and pis I , 
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while for trajectory tracking m is 2 and p is 2; that is, we have to stabilize to zero the 
two-dimensional error vector associated with the Cartesian trajectory [1]. 

2.4 CONTROL MODEL FORMULATION 

In this section we will be formulating the control model for the nonholonomic sys- 
tems. For developing the control model we will be considering the first-order kine- 
matic constraints on the system. As seen in Section 2.2, such constraints are of the 
following form: 

aJ(q)q = Q i = \,2,...,k<n 



or 

C(q)q = 0 

where vector q is the generalized coordinates, q the first-order derivative (velocities) 
of the coordinates, and C(q) the constraint matrix. 

Let us denote the set of vector fields spanning the m -dimension a I distribution A, 
which is annihilated by the constraints as g' s, j = 1,2, . . . ,m, such that 

A = span{g l ,g 2 ,...,g m ] 

The g'j s are the basis for the n — k right null space of the constraint matrix CAq) so 
that we have 

aj ( q)gj(q ) = 0; i = 1,2, ...,k< n j = 1,2, . . . , (n - k) = m (2.4) 



or 



C(q)G(q) = 0 (2.5) 

The vector fields g's are assumed to be smooth and linearly independent as 
a consequence of the assumption on aj (< j)' s being smooth and independent. By 
expressing all the feasible velocities as a linear combination of these basis vectors, 
we obtain the first-order kinematic model of the system as 

q = gM>\ + g 2 (q)v 2 + • ■ ■ + gm(q)v m (2-6) 



or 



q= X (gj(.4)vj) 

7=1 



(2.7) 
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where v' S; known as pseudovelocities, are taken as the control inputs and g'-. s are 
the input vector fields. The model directly shows the presence of k nonholonomic 
constraints on the system having n states or configuration variables and m = (n- k) 
control inputs. The control model in Equation 2.6 is known as the kinematic model 
of the system. The model is a driftless (i.e., no motion takes place under zero-input 
conditions), nonlinear, and underactuated (number of control inputs is less than the 
number of states to be controlled) control system. 



2.5 CONTROLLABILITY ISSUES 

Since the control model is driftless, the terms local accessibility and controllability 
can be used interchangeably. Moreover, the controllability of the whole configuration 
space is the (complete) nonholonomy of the kinematic constraints. The controllabil- 
ity condition can be established using Chow’s theorem. According to the theorem, 
for driftless control systems, if the accessibility rank condition 

dimA c (g 0 ) = « (2.8) 

holds, then the control system is locally accessible (controllable) from q 0 . A c is the 
accessibility distribution of the kinematic model given by Equation 2.6 and is defined 
as the span of all the input vector fields 

A c . = span{v\v e A ( -Vi > 1} 



with 



A ; =A 1 _ 1 + span{[g,v]lgeA 1 ,veA i _ 1 }, i> 2 
A l = span{gi,g 2 ,...,g m ] 



(2.9) 



This implies that A c is the involutive closure under Lie bracketing of the distribution 
A! associated with the input vector fields g. s. The term [g, v] is the Lie bracket of the 
two vector fields g and v, defined as 



[g,v](4) = !^g(4)-^v(4) (2.10) 

oq oq 

The Chow’s theorem provides both a necessary and sufficient condition for the con- 
trollability [12]. Moreover, if the system is controllable, then its dynamic extension 
given by 



<?= X (gM)vj) (2 - n) 

j = i 



© 2011 by Taylor and Francis Group, LLC 




Problem Formulation and Examples 



35 



with Vj = it j ; j = 1,2, . . . , m , is also controllable. In some cases, the use of the nil- 
potent basis is made; that is, the input vector fields g's are the nilpotent basis. 
This eliminates the need for cumbersome computations, as we will see that, using 
this concept, all higher-order Lie brackets above some particular order are zero [12], 



2.6 STABILIZATION 

The stabilization problem for the control system (Equation 2.7) can be defined as 
finding the feedback control law of the form u(q,t ) in order to make the closed-loop 
system asymptotically stable about an equilibrium point or a reference (feasible) tra- 
jectory. In the point stabilization problem, we assume an equilibrium point for the 
open-loop system, that is, q = f(q e ) = 0. 

2.6.1 Controllability and Stabilization at a Point 

For the driftless control system (Equation 2.7), any configuration ( q e ) is an open-loop 
equilibrium point under zero-input conditions. For linear systems x = Ax + Bu, it is 
a well-established fact that if the system satisfies the controllability rank condition 
given by 



rank[B AB A 2 B . . . A n B ] = n 



(2.12) 



then the asymptotic (exponential) stabilization by a smooth time-invariant state feed- 
back is guaranteed. In other words, we can say that if the controllability condition is 
satisfied, there exists a feedback law u = k(x - xj, such that the closed-loop system is 
asymptotically stable about the equilibrium point x e . 

For the control model (Equation 2.7) we would like to make a similar kind of 
analysis. For this purpose, we will look at the approximate linearization of the sys- 
tem at any equilibrium point (q e ). The approximate linearization given by 



q = 5q = G(q e )v (2.13) 

with dq = q — q e , is clearly not controllable, as the rank of the controllability matrix 
G(q e ) is m (which is less than n). Hence, we can say that a linear controller cannot 
achieve stabilization, not even locally. 

However, the controllability of the nonlinear system can be established by using 
the tools from differential geometry, that is, we can make use of the Lie algebra rank 
condition to prove its controllability. However, even if the system can be proven to 
be globally controllable (in a nonlinear sense), there is still a severe theoretical 
limitation on the point stabilization. The limitation is in a sense that a Lyapunov 
(asymptotic) stability cannot be achieved by means of a smooth time-invariant 
feedback [13]. 

The above result can be established on the basis of Brackett’s theorem [2], which says 
that the stabilization of a driftless regular system by a smooth time-invariant feedback 
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is not possible. For the driftless, underactuated control system (Equation 2.7), where 
vector fields g', s are linearly independent (regular) at q e , the theorem implies the num- 
ber of inputs m is equal to the number of states n as both a necessary and sufficient 
condition for smooth stabilization. Also, it should be noted that if the system cannot 
be stabilized by a smooth feedback, the same negative result is true for its dynamic 
extension, and also the theorem is not applicable to time-varying feedback laws. 

Thus, in order to design the feedback controllers for posture stabilization, it is 
obligatory either to give up the continuity requirement, that is, include the nonsmooth 
feedback, or to apply the time-varying laws, or a combination of both. 



2.6.2 Controllability and Stabilization about Trajectory 

In case of trajectory following, for stabilization about a trajectory it is ensured that 
the reference trajectories are feasible for the system. In other words, we should take 
or generate only those state and input trajectories that satisfy the nonholonomic con- 
straints of the system, that is, should satisfy Equation 2.2. 



2.6.3 Approximate Linearization 

For approximate linearization of the system (Equation 2.7) we take the desired state 
trajectory as q/t) and the input trajectory as vjt). It can be easily seen that the lin- 
earization about a smooth trajectory results in a linear time-varying system. The 
system can easily be shown to satisfy the controllability condition, that is, the con- 
trollability Grammian is nonsingular [21] as long as the input reference trajectory is 
persistent (does not come to a stop). Thus, it implies that we can achieve stabilization 
about the desired trajectory via a smooth time-invariant control law as long as the 
trajectories do not come to stop. One observation should be made: as we know, the 
control scheme presented here is based on the approximate linearization of the origi- 
nal system in the neighborhood of a reference trajectory; the closed-loop system is 
asymptotically stable only locally. In order to achieve global stabilization for trajec- 
tory tracking error, we have to make use of the nonlinear feedback design. 



2.6.4 Exact Feedback Linearization 

It is well known in robotics that if the number of generalized coordinates equals the 
number of control inputs, that is, n = m, the system kinematics or dynamics can be 
transformed into a linear system with the use of a nonlinear static state feedback [14]. 
The linearity is displayed by the system equations by performing some sort of coor- 
dinate transformation in the state space. 

For exact linearization of the nonlinear systems, outputs are chosen to which 
a desired behavior is assigned. Two types of exact linearization are possible. The 
two schemes are full-state feedback linearization and input-output linearization. 
In the first case, the feedback transformation is such that the whole set of system 



© 2011 by Taylor and Francis Group, LLC 




Problem Formulation and Examples 



37 



equations become linear, while in the second case, the transformation is such that the 
input and output response of the closed-loop system is linear. For multi-input-multi- 
output (MIMO) systems this transformation results in decoupling of the input and 
output vectors. Both the transformations can be achieved through static feedback or 
dynamic feedback [14]. 

2.6.5 Static Feedback Linearization 

For the nonholonomic kinematic model the full-state feedback linearization cannot 
be achieved using a static (time-invariant) state feedback. The reason for this is the 
violation of the necessary condition for the full-state feedback linearizability accord- 
ing to [14]. The fact is that the controllability condition for the system derived in 
Section 2.6 requires that the distribution A„ generated by gj is not involutive, which 
violates the necessary condition for static feedback transformation. 

However, the input-output linearization is possible with the use of static feedback. 
Here m equations are transformed via the feedback into simple decoupled integra- 
tors. However, the choice of outputs that are linearized is not unique. Here it is worth 
noticing that in the case of (2, n ) chained-form transformation, the two variables 
are indeed examples of linearizing outputs with static feedback given by the input 
transformation equation. Also, it must be noted that in the case of input-output lin- 
earization the internal dynamics may be left in the closed-loop system. Thus, for the 
exponential (global) convergence of the trajectory error to go to zero, these internal 
dynamics should be properly modeled, analyzed, and their stability guaranteed. 



2.6.6 Dynamic Feedback Linearization 

For the exact feedback linearization, if the static feedback design fails, we can also 
make use of the dynamic control feedback design for nonholonomic systems. The 
use of dynamic feedback can also result in full-state feedback linearization. For the 
model in Equation 2.7, 



q — G(q)v, qeR 11 , v e R m 



(2.14) 



the dynamic feedback compensator is of the form 

v = a(q,t,) + b(q£)r 
i = c(q,Q + cl(q£)r 



(2.15) 



where Qft) e R r is the compensator state vector of dimensions v, and r(t) e R r 
(having the same dimensions as the input vector C,(t)) is the auxiliary input. Equation 2.15 
is such that the closed-loop system obtained from Equations 2.14 and 2.15 is equivalent 
to a linear system under some state transformation z = T(q,Q- For applications to the 
nonholonomic systems, the linearization process involves the following procedure. 
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Initially, we define the output of the system y = h(q). To this output a desired 
behavior is assigned (track a trajectory). Then the output y is successively differenti- 
ated until the system inputs appear explicitly in a nonsingular way. The nonsingular- 
ity is a must for the inversion of the differentiated equations to solve for the inputs. 
If in a step involving the differentiation of system outputs the decoupling matrix 
(differential map) of the system is singular (which means that some input is still not 
appearing), integrators are added on some of the input channels and the process of 
differentiation is continued. It is also necessary to avoid the direct differentiation of 
the system inputs in the next differentiation. This operation is known as dynamic 
extension and converts a system input into a state of dynamic compensator. The 
dynamic compensator has the new auxiliary input r as its input. The process of 
differentiation continues until at some point the system is invertible (i.e., a solution 
for new inputs can be obtained) from the chosen output vector y and the process 
terminates. The number of successive addition of integrators gives dimensions of 
the state £ of the dynamic compensator. Also, if the sum of the orders of the output 
differentiation is equal to the dimensions of the extended state space system (original 
and dynamic compensator state), which is n + v, then the full-state linearization of 
the system is obtained, as there are no internal dynamics left in the system. 



2.7 EXAMPLES OF NONHOLONOMIC SYSTEMS 

The simplest example of a nonholonomic system can be a wheel that rolls on a lane 
surface, such as a unicycle. The constraints here arise due to the roll without a slip 
condition. The configuration or the generalized coordinate vector is q = (x, y, 0). The 
coordinates x and y are the position coordinates of the wheel, and 0 is the angle that 
the wheel makes with the x axis. The unicycle is shown in Figure 2.2. The constraint 
here is that the wheel cannot slip in the lateral direction. 

The generalized velocities are subject to the following kinematic constraint: 

isin0- ycos0 = O (2.16) 

In other words, the velocity along the plane perpendicular to the point of con- 
tact between the wheel and the ground is zero. The above equation is of the form 
C(q)q = 0, with constraint matrix C(q) = [sin0 - cos0 0]. 




FIGURE 2.2 The nonholonomic constraints on a unicycle. 
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FIGURE 2.3 The nonholonomic constraints on a car-like robot. 



Expressing the feasible velocities as a linear combination of vector belds span- 
ning the null space of the matrix C(q), we get the following kinematic model: 

q = gi(q)v 1 + g 2 (q)v 2 



( . \ 
X 




cos0 'j 




f°l 


y 


= 


sin0 


Vi + 


0 


v ® J 




l 0 J 




lu 



where v, is the linear velocity of the wheel and v 2 is its angular velocity around the 
vertical axis. Here we observe that the number of states n = 3, number of control 
inputs m = 2, and number of nonholonomic constraints k= 1. 

Another example is that of a car-like robot shown in Figure 2.3. The robot has 
two wheels, and each wheel is subject to one nonholonomic constraint. The con- 
straint is the same as in the case of a unicycle. The generalized coordinate vector is 
q = (x,y,0,(|)), with x, y, and 0 the same as before. The angle ( |) is the steering angle. 

The two nonholonomic constraints on the front and rear wheels, respectively, are 



isin(0 + (|)) — ycos(0 + ((>) — 0/ cos(|) = 0 
xsin0-ycos0 = 0 



(2.18) 



Here / is the distance between the wheels. Again, this is of the form C(q)q = 0, with 



C(q) = 



sin(0 + (|)) 
sin0 



- cos(0 + c()) 
— cos 0 



— / cos cf) 0 

0 0 
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Choosing rear wheel drive, the kinematic model is obtained as 



<1= gM)Vi+ g 2 {q)v 2 



( . \ 
X 




cos0 'l 
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A) 




v 0 J 




lu 



(2.19) 



Here v, is the rear driving velocity input and v 2 is the steering velocity input. The 
above model is not defined at cf» = jt/2, where y, is discontinuous. Physically, this 
corresponds to the car becoming jammed because of its front wheel being normal to 
the axis of the body. 

The feedback control design, controllability analysis, and motion planning for 
all three motion tasks are done in [1], For a detailed analysis, the reader is referred 
to the same. Another example of nonholonomic systems is that of an underwater 
vehicle, which is discussed in the next chapter. In Chapter 3 we will study the motion 
planning for the same, and controllability of the system is discussed and proved. We 
also present feedback control laws that give global stabilization of the vehicle about 
a desired trajectory and about a point. 
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In this chapter an overview of an underwater vehicle is given and a mathematical 
model of the vehicle is derived. The chapter presents in detail the derivation of the 
mathematical modeling of the system. For motion planning tasks the kinematic 
model of the system is obtained and the issues related with nonlinear controllabil- 
ity of the system are studied in detail. Finally, for the purpose of control design, the 
system is converted into a chained form. 



3.1 MATHEMATICAL MODELING 

In this section the mathematical model of an underwater vehicle is briefly discussed. 
An underwater vehicle is generally a six degrees of freedom body. It follows the laws 
of rigid body motion. The dynamics of the system are highly nonlinear due to rigid 
body coupling and hydrodynamic forces on the vehicle. The mathematical model of 
the underwater vehicle is obtained through the following two models: 

Dynamic model: This type of model allows for the actual forces, causing 
the motion and the dynamic properties of the vehicle to be taken into 
account. The equations of translation and rotation are obtained using 
Newton’s law [15]. 

Kinematic model: The kinematic model of the system is where the actual 
forces causing the motion and the dynamic properties of the vehicle do not 
enter the equations of motion. This type of model allows for the decoupling 
of the vehicle dynamics from its movement. An autonomous underwater 
vehicle has a nonholonomic nature due to its nonlinear kinematic model 
[22] . In the following section the kinematic model of the vehicle is derived. 
For the rest of this chapter and Chapter 4, the kinematic model of the sys- 
tem will be used for analysis and control purposes. Chapter 5 will introduce 
the dynamic model. 



3.1.1 Kinematic Modeling and Nonholonomic Constraints 

The kinematic model of the system is obtained by taking into consideration the non- 
holonomic constraints on the linear velocity. The nonholonomic constraints restrict 
the velocity of the system to be zero in certain directions, but these restrictions do 
not restrict the global movement of the system. Such constraints can arise when two 
surfaces roll against each other or in space-based systems where the total angular 
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FIGURE 3.1 The coordinate systems of an underwater vehicle. 



momentum of the system is conserved. For the development of the kinematic model 
of the underwater vehicle, we assume two orthogonal coordinate systems [23]: 

Global coordinates: The global or the inertial frame coordinates are denoted 
by ( O , X, Y, Z). The frame remains fixed at the ocean surface with origin O. 
The unit vector in the Z direction points down into the water, while the unit 
vectors along the X and Y direction complete the right-handed system. 

Local coordinates: The local or the body frame coordinates are denoted by 
(p, x, y, z). The frame remains fixed on the vehicle surface with origin p. 
The two coordinate systems are as shown in Figure 3.1. 



3.1.2 Kinematic Model with Respect to Global Coordinates 

The kinematics of the vehicle is described by six state variables and four input vari- 
ables. The kinematic relationships describing the transformations between the two 
coordinate systems can have a number of parameterizations. The one used here is the 
Euler angle parameterization [25]. In the Euler angle representation the orientation 
between the inertial and the local coordinate frame is expressed in terms of a sequence 
of three rotations, roll cf), pitch 9, and yaw \|/, about the axes x, y, and z, respectively. 

Let q = [p r )] 7 be the vector of six generalized coordinates required to specify the 
kinematics of the vehicle. The six coordinates are the Cartesian coordinate vector p = 
[x, y, z\ T of the vehicle in the local frame and the orientation coordinate vector q = 
[<]), 9, \|/] r . The orientation vector is the vector of Euler angles that gives the orientation 
of the body frame with respect to the inertial frame. The transformation from the local 
coordinate frame to the global coordinate frame is given by means of a transformation 
matrix R e S(03) called rotation matrix, where S(03) is a group of rigid body rotations. 
The matrix R is an orthogonal matrix satisfying the relation RR T = /, that is, R T = R~ l and 
det( R) = 1. The matrix R is given as R= [n 5 a] with the column vectors being 



hi 


r l2 


r l3 


hi 


r 22 


r 23 


hi 


r 32 


r 33 



(3.1) 
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with the matrix entries being 



r n = cos0cos\|/ 

r l2 = sin 0 sin 4> cos \|/ - cos (|> sin \|/ 
r 13 = sin 0 cos 4> cos \)/ 
r 2l = cos 0 sin \j/ 

r 22 = sin 0 sin (|) sin \|/ + cos (|) cos \)/ 
r 23 — sin 0 cos c|) sin \|/ — sin c|) cost)/ 
r 3 i = -sin0 
r 32 = sincf) cos0 
r 33 = cos (|> cos 0 



(3.2) 



Let v = [v x 0 0] r be the linear velocity of the vehicle, that is, the vehicle has 

linear velocity along the x axis only, and let CO = [co r CO co,] r be the angular 
velocity components along the axes x, y, and z, respectively in the body frame. The 
velocity vector along the three coordinate axes and the time derivative of the Euler 
angles are obtained from the following relations: 



p = Rv = [n s fl]v 


(3.3) 


R = RS(03) 


(3.4) 



where 5(co) is the skew-symmetric matrix given as 



S( C0) = 



0 




CO, 




0 


-CO, 


-co. 


co v 


0 



The above equations give the following on solving 

/) = y 1 (r0v,ii = / 2 (ri)co (3.5) 



with 



/[(/!)= [cos 0 cost]/ cos0sin\|/ -sin0] r 



J 2 (r[) = 



1 

0 

0 



sin(f>tan0 cos(|)tan0 
cost)) — sin cf) 

sin (|) sec 0 cos 4> sec 0 
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The above set of equations can be written as the following equations: 



x(t) = r n v = cos\|/cos9v 

y(t) = r 2l v — sin \|/ cos 0 v 

z(t) = r 3l v = - sin 0v 

<j)(f) = co v + sin ()) tail 00 ), + cos(|)tan0co z 

9 (t) = cos(])co v - sincfxo, 

\j /(f) = sin(|)sec0co T + cosc|)sec0co. 



This can be written in matrix form as 



x(t) 




COS\(/COS0 


0 


0 


0 


m 




sin\)/ cos0 


0 


0 


0 


z(t) 




-sin0 


0 


0 


0 


fct) 




0 


1 


sin 4> tan 0 


cos(f>tan0 


0(0 




0 


0 


COS(|> 


— sincf) 


v(0 




0 


0 


sinc|)sec0 


cos (|) sec 0 



COy 



( 3 . 6 ) 



( 3 . 7 ) 



The equations can be written in generalized vector form as 



x(t) 


r 


)’(t) 




z(t ) 




<ko 




0(0 




v(0 





COS\|/COS0 




0 




0 




0 


sin \)/ cos 0 




0 




0 




0 


-sin0 

0 


v + 


0 

1 


co,+ 


0 

sin (|> tan 0 


C0 y + 


0 

cos(|>tan0 


0 




0 




COS(|) 




— sin <j) 


0 




0 




sin(|)sec0 




cos (|> sec 0 



( 3 . 8 ) 



The system here is subject to two nonholonomic constraints. The constraints are 
on the linear velocities along the y and z directions. The velocities along these direc- 
tions are zero. The two constraints are as 



s T p = 0 
a T p = 0 

which can be written as 

r u x+r 22 y+r n i = Q 
r l3 x + r 23 y+ r 33 z = 0 



( 3 . 9 ) 
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Alternately, these equations can be written as 

(cos \|/ sin 0 sin (|> - sin\|/cos(|))i + (sin \j/ sin 8 sin 4> + cos \|/ cost))) y + (cos Osin 4>)j = 0 
(sin \|/ sin 0 cos cf> — sin \)/ sin §)x + (sin \|/ sin 0 cos 4> - cos \]/ sin <|>) y + (cos 0 cos 4>) z = 0 



The above equations are of the form A(q)q = 0, with 



Mq) = 



r l2 r 22 r i2 0 0 0 

t*13 r 23 r 33 0 0 0 



(3.10) 



Expressing the feasible velocities as the linear combination of vector fields g^q), 
g 2 (q), g 2 (q), and g A (q) spanning the null space of matrix A(q), we have the following 
kinematic model: 



<?(0 = gi(q)v i + g 2 (q)v 2 + g 2 (q)v 2 + g 4 (<?)v 4 



q(t) = [gi(q) g 2 (q ) g 3 (q) g* (q)\ 



Vi 

v 2 

v 3 

V 4 



(3.11) 



with Vj = v x ; v 2 = co t ; v 3 = co v ; v 4 = co. and 

g\(q) = [cos 0 cost)/ cos 0 sin \|/ -sin0 0 0 0] r 
g 2 {q) = [ 0 0 0 1 0 OF 

(3.12) 

g 2 (q) = [0 0 0 sin(|)tan0 cosc|) sinc^secO] 7 
<? 4 (<7) = [0 0 0 cos(|)tan0 — sin cf) cosc|)sec0] r 

More generally we can write 

q = G(q)v (3.13) 



The above equations are the kinematic model of the system. The system is non- 
linear and underactuated, which means that the number of inputs to the system is 
less than its states. The generalized velocity vector q cannot assume any independent 
value unless it satisfies the nonholonomic constraints. The constraints are examples 
of the Pfaffian constraints, which are linear in velocities. The admissible generalized 
velocities as given by Equation 3.1 are contained in the null space of the constraint 
matrix A(q). 
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3.2 CONTROLLABILITY ANALYSIS 

Consider the system in Equation 3.13, 

q = G(q)v 

The system is nonlinear, underactuated, and driftless; that is, no motion takes place 
under zero-input conditions. Thus, in order to establish the controllability of the 
vehicle, we make use of the mathematical concepts that are involved in the Lie alge- 
bra rank condition and differential geometric control theory concepts. 

3.2.1 Controllability about a Point 

Consider the linear approximation of the system (Equation [3.13]) at equilibrium point q e 
while letting the input v e equal zero. Let the error associated with the equilibrium point 
be given as 



q = q-q e 

The time derivative of the error is given as 

q = gMe)V\ + g 2 (q e )v 2 + g 3 (qe)v 3 + gMe)V 4 



or 

~q = G{q e )v (3.14) 

Here G(q e ) is the controllability matrix at the equilibrium point. The rank of the con- 
trollability matrix is 4. Thus, if we linearize the system about an equilibrium point, 
the linearized system is not controllable. Hence, the linear controller will not work 
here. To test the controllability of the above system, we make use of the Lie algebra 
rank condition and nilpotent basis: 

Nilpotent basis: The definition of nilpotent basis for a distribution is recalled 
here [16]. Given a set of generators or basis vector fields g u g 2 ,g 3 ••• , g m , 
we define the length of a Lie product recursively as 

Hgj] = 1, i = l,2, ...,w 

l{[A,B]} = l[A\ + l[B] 

where A and B are themselves Lie products. Alternatively, /[A] is the number 
of generators in the expansion for A. The Lie algebra or basis is nilpotent if 
there exists an integer k such that all Lie products of length greater than k 
are zero. The integer k is called the order of nilpotency [17]. The use of the 
nilpotent basis eliminates the need for cumbersome computations, as we see 
that all higher-order Lie brackets above some particular order are zero. 
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In the light of the above definition and conditions, we see that the Lie algebra 
L{gj, g 2 , g 3 , g 4 } is a nilpotent algebra of order 2 (k= 2); that is, the vector fields g v g 2 , 
g 3 , and g 4 are the nilpotent basis. Thus, all Lie brackets of order more than two are 
zero. The only independent Lie brackets computed from the four basis vector fields 
are [ gl , g 3 ] and [g v gj. 

Thus, for our system the Lie algebra rank condition becomes 



rank[C c ] = 6 



or 



rank[g l ,g 2 ,g 3 ,g 4 ,[g 1 ,g 3 ][g 1 ,g 4 ] = 6 (3.15) 

where [g 3 , g 3 ] and [g,, g 4 ] are two independent Lie brackets computed from the four 
vector fields [g 1; g 2 , g 3 , g 4 ] per the following definition: 

, , , . . dh dg , 

[g,h](x) = J *~g-^-h (3.16) 

OX ox 



Thus, we have 



[&>&](*) = ^p-gi-^r L & 

dx dx 

= [cos\|/sin0cosc|H-sin\|/sin(() sin V|/ sin 0 cost]) — cost)/ sin cp 
cos0cos(|) 0 0 0] r 

[,?i,g 4 ]W= g, - ^f‘ §4 

dx dx 

= [-cos\|/sin0sin(|) + sin\|/cosc|) -sin\)/sin0sin(|)-cos\|/cos(|) 
-cos0sinc|) 0 0 0] r 

Using the above expressions for the Lie brackets, the controllability matrix C c 
becomes 



C = 



cos\|/cos0 0 0 0 

sin\|/cos0 0 0 0 

-sinO 0 0 0 

0 1 sincJitanO cos<()tan0 

0 0 cos(|) — sin <() 

0 0 sin (|) sec 0 cos<()sec0 



cos \|/ sin 0 cos c|> + sin \|/ sin <|> 
sin \|/ sin 0 cos $ - cos \|/ sin c|> 
cos 0 cos (]) 

0 

0 

0 



cos \|/ sin 0 sin (|) + sin \|/ cos (]) 
sin \|/ sin 0 sin $ - cos \|/ cos (]> 
-cos0sin(() 

0 

0 

0 
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The controllability matrix C c has one nonzero minor of order six. Thus, the rank of 
the controllability matrix is full as long as 0 ^ n/2, which is the singularity of the 
system. Hence, we conclude that the system is controllable locally and also globally 
as long as it avoids the singularity condition. 



3.2.2 Controllability about a Trajectory 

For the nonlinear system 



q = G(q)v 



let the reference state trajectory be q d {t)= [x d (t),y d (t),z d (t),$ d (t),Q d (t),\\r d (t)] T and 
the reference input trajectory be v d (t) = [v (/1 (r), v d2 (t), v di (t),v dA (t)] T . The reference 
trajectory should satisfy the nonholonomic constraints on the system. 

For the linear systems x(t) = Ax(t) + Bu(t) controllability implies asymptotic 
(actually exponential) stability by smooth state feedback. Thus, if the accessibility 
rank condition rank [B,AB,A 2 B, ... ,A n ~ l B~\ = n is satisfied, then there exists a feed- 
back gain so that the control law 



u(t ) = k(x d - x) 



makes the desired trajectory x d (t) asymptotically stable, or in other words, the error 
associated with the desired solution goes exponentially to zero. 

For nonlinear systems the condition does not apply as such. But for local acces- 
sibility we may look at the approximate linearization of the system in the neigh- 
borhood of x d (t). Thus, in particular, if the linearized system is controllable, the 
nonlinear system can be stabilized locally at x/t) by a smooth feedback u(t) = kx e . 
The condition is sufficient but not necessary. 

Let the errors associated with the desired state trajectory and input trajectory 
be denoted as q e (t) = q(t) - q d (t) and v e (t) = v(f) — v d {t), respectively. Linearizing the 
system about the desired trajectory we obtain the following system: 



q(t) = q d (t) + q e (t) 

= {G{q d + q e ,t)}{ v d ( t ) + v e (t ) } 



(3.17) 



The Taylor series expansion of G(q , t) about the nominal solution q d (t) is given as 
<7(0 = {G(q d ,t) + ( dG{q)/dq \ q=qd )q e {t) + h.o.t. } {v d ( t ) + v e (t) } 

Since the nominal solution satisfies Equation 3.16, we have 

q e (t) = {dG(q)!dq \ a=OJ q e (t)}v d (t)+ G(q d ,t)v e (t) 
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or 



q e (t) = A(t)q e (t)+ B(t)v e (t) 



where 



Mt) = dgjdq \ q=qd )v di (t) 

i = 1 



and 



B(t) = G(q d ,t ) 



(3.18) 



Upon computations we get 



A(t) = 



£*3x3 

£*3x3 



Mt) 

Mt) 



with 



Mt) = 



o 

0 

0 



- cos \|/ rf sin 9 d v dl (f ) 

— sint) f d sin Q d v dl (t) 

cos Q d v dl (t) 



- sin \|/ rf cos 0^(0 
cosy d cos Q d v dl (t) 
0 



and 



Mt) = 



cos<t> rf tan 9 d v d3 (0 — sin tan 9 d v iA {t) 
— sm§ d v d3 (t)- c°s$ d v d4 (t) 
-cos^ sec 0 d v d3 (r)- sint) sec 0 d v d4 (f) 



sinc) rf sec 2 Q d v d3 (t) + cost^ sec 2 9 d v d4 (t) 0 

0 0 

sint^ sec©^ tan 6 d v d3 (t) + cost ) d sec0 rf tan0 d v d4 (f) 0 



and 



B(t) 



J 1 (0 £*3x3 

£*3x1 J 7 .it) 
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where 



and 



■A (0 = 



cos \|/ rf cos Q d 
sin\]/ rf cosG^ 
- sin 6 d 



J 2 (t) = 



1 sin 4>^ tanO^ 
0 cos§ d 
0 sin 4>^ sec d d 



cos tan 9 d 
~ sin 

cos (|) d sec Q d 



The above system is linear time varying, but for a linear reference trajectory with 
constant velocities v di (t) = v dn and q d ( 0) = q( 0) the controllability condition becomes 

rank [B,AB,A 2 B,A 3 B, A 4 B , A 5 B] = 6 



Upon computations we see that the above matrix has a nonzero minor of order six 
as long as v dl = v d3 = v d4 ^ 4 and 9 d ^ n/2. Thus, the linearized system is controllable 
along a reference trajectory as long as the trajectory does not collapse to a point. 

3.3 CHAINED FORMS 

The chained-form systems were hist introduced in [18]. The chained form introduced 
in [18] had one chain, that is, two input chained forms. The method for converting 
the multi-input drift-free nonholonomic systems into chained forms is given in [19] 
and [20]. The chained form thus obtained has more than one chain. The method 
presented in [19] for transforming is similar to the method of exact linearization 
of nonlinear systems with drift via state feedback, as presented in [14]. The same 
method will be applied for our system with some modifications. 

The system 



q(t) = gM)v\ + g 2 (q)v 2 + g3(q)v 3 + g 4 {q)v 4 



can be transformed into the chained form by a feedback transformation. In the above 
equation, with g : being smooth and linearly independent vector fields, there exists a 
feedback transformation (q. a, ij, y) = ® (q) and v = (3 (q) that transforms the system 
into the following chained form [21]: 



^ — U ] , O.q — U 2 , Tfo — M 3 >Yo — M 3 

'Ho = a 0 Mi,fii = ri oMl 



(3.19) 
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There exists a basis function /, , f 2 , / 3 , f 4 , for the distribution A 0 = span(g l ,g 2 ,g 3 ,g 4 ), 
having the form 




d_ 

<kt 





(3.20) 



The basis functions are chosen such that the following distributions 

G 0 = span{f 2 ,f 3 ,f 4 ) 

G, = span(f 2 J 3 ,f 4 ,[f,f 2 \,[/'.m/,f 4 ]) 



with 



G 5 = span | ad'f. f 2 , ad‘ fi / 3 , ad j-. f 4 ); 0 < i < 5 



ad k fi f 2 : = [f \,ad k f ~ l f 2 \ and ad° fi f 2 . = f 2 



have constant dimension on the same open set U e R”, are all involutive, and G 5 has 
dimension five on U. 

The vector fields f v f 2 ,f 2 , and f 4 which satisfy these conditions, are 



f = —^— = [ 1 tanti/ -tanOsecti/ 0 0 0] r 

C\|/C0 

(3.21) 

fi = 82ifi = 83 >/r = 84 



The coordinate transformation for the system thus is 

^0 = a o = L\h,, r| 0 = y 0 = h 4 

®1 = ^* 2 , Hi = ^3 



(3.22) 



© 2011 by Taylor and Francis Group, LLC 




52 



Autonomous Underwater Vehicles 



where h v h 2 , h 3 , h 4 , are the smooth functions such that the following conditions are 
met: 

dh x 1 Gy, 0<j<5 

and the distribution G 0 is annihilated by dh x , dh 2 , dL° f Ji 2 , dl} f h 2 , dh 3 , dL° f Ji 3 , dD f h 3 , 
and dh 4 . Here L f h 3 is the Lie derivative of h 3 with respect to /]. For the detailed proof 
of the above conditions, refer to [19]. Here it should also be noted that the choice of 
functions h t ,h 2 ,h 3 , h 4 is not unique. Choosing 

h x = x , h 2 = y, h 3 = z 

and 



with R being the rotation matrix and trace(R) = ( r M + r 21 + r 33 ). Thus, the coordinate 
transformation for the system becomes 

x 1 = ^ 0 = x 

x 2 =a 0 = tan\)/ 



x 3 = a 1 = y 



x 4 = T | 0 = _ tan 9 sec ¥ 



(3.23) 



x, = z 



Xf, = (r 32 - r 23 ) 

6 1 + tr(R) 3 23 



which gives the following chained-form system: 

ij = u x 
x 2 = u 2 
x 3 = x 2 u l9 

x 3 = x 2 u x , (3.24) 

x 4 = u 3 
x 5 = x 4 u x 
x 6 -u 4 
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Solving Equations 3.23 and 3.24 and inserting Equation 2.8, we get the input 
transformation as 

tq = x x 
u 2 = x 2 
m 3 = i 4 
m 4 = x 6 



or 

Mj = COS\|/COS0Vi 

u 2 = sec 2 \|/sin(|)secc0V3 + sec 2 \|/cosc|)sec9v 4 



(- sint)/ sin (|) sin 0 - cos \|/ cos (|)) (sin \|/ cos <f» sin 0 + cos \|/ sin (()) 

U 3 — v 3 v 4 

COS 2 \|/COS 2 0 COS 2 \|/COS 2 0 

(1+ cos \|/ cos 9) v 2 + (cos \|/ sin 0 sincf) — sin \|/ coscf)) v 3 + (sin \|/ sin 0 cos (f» + sin \|/ sin(f»)v 4 



1 + cost)/ cos 0 + sin\|/ sin 0 sin (|) + cos\|/ cosc|) + cos 0 cos (|) 



(3.25) 



which gives 
v x = V'! = Mj/costi/ COS0 

C0 V = v 3 = cos\|/cos0{(cos\|/sin(f>-sin\|/sin0cos(|))M 2 — (cost)) cos 0 )m 3 } 

CO, = v 4 = cos \|/ cos 0{ (sin \(/ sin 0 sin c(H- cost)/ cos 4>)t< 2 — (sin 4> cos 0)w 3 } 

COy = v 2 = [(1 + cos\|/ cos 0 + sin \|/ sin 0 sin 4> + cost)/ cos 4> + cos 0 cos (|))w 4 ... 

cost)/ cos 0 

- (cos \|/.?0 sin cf> — sin \|/ cos 4>)v 3 - (sin \]/ sin 0 sin cf) + sin \|/ sin 4>) v 4 ] 



The inputs v h v 2 , v 3 , v 4 can be calculated from the above equations provided 
cos\(/cos0 A 0. Also, here it should be noted that the chained-form system is com- 
pletely controllable, as the controllability is not affected by state feedback and coor- 
dinate transformations, that is, they are invariant under the transformations. The 
chained form thus obtained is given as 



X x — X x = Mj, x 2 = x 2 = u 2 , x 4 = x 3 = m 3 , 
x 6 = x 4 = n 4 , i 3 = x 20 = x 2 u u x 5 = x 31 = x 4 u x 



This form will be used for the control design, which is discussed in the next chapter. 



© 2011 by Taylor and Francis Group, LLC 




Control Design Using 
the Kinematic Model 



In this chapter, the controllers will be designed for the vehicle to track a desired tra- 
jectory, follow a path, and for point-to-point stabilization. The chapter presents the 
control design and the simulation results obtained for the model of an underwater 
vehicle developed in Chapter 3. The feedback control design is developed using the 
kinematic model of the system. The performance of the controllers obtained using 
various techniques of control design are evaluated for different motion planning 
tasks mentioned above. The chapter also presents the simulation results obtained for 
different controllers. The simulation results are used to compare and evaluate the 
performance of the various controllers. 



4.1 TRAJECTORY TRACKING AND CONTROLLER 
DESIGN FOR THE CHAINED FORM 

The system is supposed to track a given (desired) Cartesian trajectory. The prob- 
lem is to regulate both the vehicle’s position and orientation with respect to that of 
a reference system, the trajectory of which is parameterized by the variable t. The 
goal will be achieved using the feedback control law with the following control 
schemes: 

• Full-state feedback using approximate linearization 

• Feedback linearization using input-output linearization or full-state linea- 
rization 

Before going for the feedback design, the problem of generating the desired 
output trajectory is discussed for both the original system and the chained-form 
system. 



4.2 REFERENCE TRAJECTORY GENERATION 

Let the reference state trajectory be q d (t) = [x d (t),y d (t),z d (t),§ d (t),Q d (t),\\r d (t)] T and 
the reference input trajectory be v d (t) = [v dl (t), v d2 (t), v d3 (f), v d4 (f)] r . The desired tra- 
jectory is feasible only when it satisfies the nonholonomic constraints on the system. 
Assume that a feasible and smooth desired output trajectory for the chained form 
is given as [x dl (t),x i3 (t),x d5 (t),x i6 (t)] r x d (t). From this information we are able to 
derive the time evolution of the rest of the coordinates of the state trajectory and 
the associated input trajectory. In other words, we should be able to recover the 
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state trajectory and the input trajectory from the reference output trajectory. From 
Equation 3.24 we have 

x d i (0 = u dl (t) 
x d2 (t) = u 2d (t) 

x d3 (t) = x d2 (t)u <n (t) (4]) 

x d4 (t) = u d3 (t) 
x d5 (t) = x d4 (t)u dl (t) 
x d6 (t) = u d4 (t) 

with initial conditions of the states as [x dl (t 0 ),x d2 (t 0 ),x d3 (t 0 ),x d4 (t 0 ),x d5 (t 0 ),x d6 (t 0 )] r 
at 1- 1 0 . Solving for the state trajectory from Equation 4.1 we get 

x d2 (t) = x d3 (t)/x d i(t) 
x d4 (t) = x d5 (t)/x dl (t) 



The corresponding input trajectory is given as 

u d i(t) = x dl (t) 

u d2 (t) = x d2 (t) = (x dl (t)x d3 (t) - x d3 {t)x dl {t))/x 2 dl (t) (4 

u di (t) = x d4 (t) = (x dl (t)x d5 (t)~ x dS (t)x d i(0)/iJi(0 
u d4 (t) = x d6 (t) 

Equations 4.2 and 4.3 give the unique state and input trajectory from which the 
desired output trajectory can be reproduced or generated. As is seen, the values 
of the trajectories depend upon the values of the output trajectory and its second- 
order derivatives. Thus, the output trajectory should be differentiable everywhere. 
The derivation of the reference input and state trajectory, which generates a desired 
output trajectory, can also be performed on the original system. The original state 
and input trajectories can be derived from the output trajectory as 

x d (t) = x dl (t) 

y d (t) = x d3 (t) 

z d (t) = x d5 (t) ( 4 . 4 ) 

y d (t) = tan-'(x d2 (f)) = l-dn-'(y d (t)/x d (t)) 

Q d (t) = - tan- 1 (x d4 (f)cos\|/ d (f))-- tan" 1 (i d (f) cost) f d {t)/x d (t)) 

4 )rf (0 = cot- 1 (cot e rf (0/sin V|/ rf (t) + tan\|/ d (0/sin Q d (t)) 
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Similarly, the actual input trajectory is 

Vdi 0 ) = y]xj + yd+Zd = MO/O11W 

v d 3 (0 = “On (t)(r d23 (t)u d2 (t) + r d33 (t)u d3 (t)) 

v d4 (t) = r dn (t)(r d22 (t)u d2 (t) + r d32 (t)u d3 (t)) (4.5) 

v d2 (t) = - — + 

1 + On (t) 

with 

r dn = cosd d cos\| i d 

r d 12 = sin Q d sin <|o cost \i d - cost)),, sin \|/ rf 
O13 = sin 9 ^ cos<\> d cost) f d + sin<|o sin \[i d 
02i = cos e d sin \\) d 

r d 22 = sin 0 rf sin (|) rf sin t^ + cos <0 cos \|/ rf (4.6) 

0 23 = sin 0^ cosc|o sin\|/ rf -sinc|o cos\|/ f , 

Osi = - sin 0 d 

0 32 = sin <10 cos 0 rf 

033 = cos <t>^ COS 0 rf 

For the tracking simulation purposes consider the following reference sinusoidal 
output trajectory: 



x d \(t) = t, x d3 (t) = Asincor, x d5 (t) = 1, x d6 (t) = 0 (4.7) 



This gives the state trajectory as 



x d2 (t) = Aco cos cor, x dA (r) = 0 



(4.8) 



and the input trajectory as 



u dl (r) = 1, u d2 (t) = - Aco 2 sin cor 



u d3 (t) = 0, u d3 (t) = 0 



(4.9) 
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The initial states are 



W°) = ° W°) = Aco W°) = 0 

•WO) = 0 x d5 (0) = 1 x d6 (0) = 0 



( 4 . 10 ) 



Here again it is to be noted that there is a singularity in the state and input trajectories 
at x dl (t) = 0 or u dl (t) = 0, as the state and input trajectories are not defined at that 
point. 



4.3 CONTROL USING APPROXIMATE LINEARIZATION 

The feedback controller for trajectory tracking is based on standard linear control the- 
ory. The design makes use of the approximate linearization of the system equations 
about desired trajectory, which leads to a time -varying system, as seen before. The 
method here is illustrated for the chained-form equations about the desired trajectory. 
The chained-form system is linear under piecewise constant inputs. For the chained- 
form system the desired state and input trajectory computed in correspondence to the 
reference Cartesian trajectory is 



X <1 ( t ) = {x di (t), X d2 (f), X i 3 (t),X d 4 (t),X d 5 (t),x d6 (t ) ! 



and 



u d (t) = [u dl (t),u 

d2 (0, U d3 OX U d\ 0)} 



( 4 . 11 ) 



An equivalent way to state the tracking problem is to require the difference 
between the actual configuration and the desired configuration to approach to zero. 
This difference is denoted as the error. Since the vehicle will not necessarily share 
the same initial conditions as the desired system, the tracking controller will drive 
the error to zero and minimize the effect of the disturbances as the vehicle converges 
to the reference trajectory. 

In order for the system to track the trajectory, this error should approach zero with 
time. Denoting the error variables for states and inputs as following 

x e = x-x d 



and 



u e = u-u d 



( 4 . 12 ) 



© 2011 by Taylor and Francis Group, LLC 




Control Design Using the Kinematic Model 



59 



the error differential equations are written by subtracting the desired equations from 
the system (actual) equations as the following nonlinear set of equations: 



X eI 


= «rf 




*e2 


= u e2 




x e3 


= X 2 U\ - 


X d2 U dl 


X e 4 


= Ue 3 




X e 5 


= x A u^ - 


- X dA U dl 


X e6 


= U eA 





(4.13) 



Now linearizing about the desired trajectory, we have the following linear system: 

(4.14) 



0 

0 

0 

0 

0 

0 



The system given by Equation 4.14 is linear time varying and can easily be proven 
to be controllable by checking its Grammian [21] to be nonsingular. For a linear tra- 
jectory with constant velocity u dl (t ) = u di the controllability condition is given by 

rank{B,AB,A 2 B,A 3 B,A 4 B,A 5 B) = 6 (4.15) 

The matrix in Equation 4.15 is a nonsingular matrix and has at least one nonzero 
minor of order six. The controllability matrix is nonsingular only as long as the input 
u dl to the system is nonzero. This corresponds to the singularity in the kinematic 
model of the system. Thus, the system is controllable as long as u d\ ^ Choosing the 
linear time-varying feedback law for the system as 

u e =-Kx e (4.16) 



xjt) = A(t)x e (t)+ B(t)u e (t) 



with 



A{t)=\ 



0 

0 

0 

0 

0 

0 



0 

0 

u dl (t) 
0 
0 
0 



0 

0 

0 

0 

u dl (t) 



and 



B(t) = 



1 

0 

x d2 {t ) 

0 

x dA (t) 
0 
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for the chained-form system the control law should be such that the feedback law for each 
chain contains the same number of terms as the number of states in that chain. Thus, 



U el = ~ k \ X e 



u„2 = ~k 2 x e 2 X. 



U e 3 = - k 4 X e4 * e 5 



= ~k 6 x e6 



(4.17) 



The feedback coefficients k 3 and k 5 are divided by u dl so that the characteristic 
equation of the closed-loop system matrix does not contain u dl , thus making the 
design global. Thus, the matrix K is given as 



K 

0 

0 

0 



0 0 0 0 

k 2 k 3 Xi d | 0 0 

0 0 k A k 5 /u d | 

0 0 0 0 



0 

0 

0 

k 6 



(4.18) 



The k's are chosen such that k x and k 6 are positive, and k 2 , k 3 , k A , and k 5 are such that 
X 2 + k 2 X + k 3 and X 2 + k A X + k s are Hurwitz. The closed-loop system matrix is thus 
given as 

A c i = A - BK 





0 


0 


0 


0 


0 


0 


~k 2 


— k 3 /u dl 


0 


0 


0 


x d2 k l 


11 dl 


0 


0 


0 


0 


0 


0 


0 


—k A 


~k 5 /u dl 


0 


x dA k x 


0 


0 


"dl 


0 


0 


0 


0 


0 


0 


0 


K 



(4.19) 



The closed-loop system matrix has constant eigenvalues with negative real parts. 
This does not guarantee the asymptotic stability of the closed-loop time-varying 
system [26]. However, for specific choices of u dl (t), bounded away from zero and 
u d2 (t), u d3 (t), and u dA {t ) = 1, it is possible to use results on slowly varying linear 
systems in order to prove asymptotic stability. The feedback matrix is obtained by 
using the pole placement technique. Since for stability the eigenvalues of the closed- 
loop matrix should have negative real parts, the characteristic equation of the system 
should satisfy the following: 

det(s/ -A c ,)=(s- p 3 )(s- p 2 )(s- p 3 )(s- p A )(s - p 5 )(s - p 6 ) 



© 2011 by Taylor and Francis Group, LLC 




Control Design Using the Kinematic Model 



61 



where p r i = 1,2, ...,6 are the eigenvalues of the system. The resulting closed-loop 
system (Equation 4.19) is controllable with the choice of feedback in (Equation 4.17). 

4.3.1 Simulation of the Controller 

For simulation, the following sinusoidal trajectory is chosen: 

x d (t)=t y d (t) = a sin cot z d (t) = 1 

which gives the desired values for the chained-form states and inputs as follows: 
x d\(t) = t x d2 (f) = a(» cos m x di (t) = a sin cot 
x d4 (t) = 0 x d5 (t) = 1 x d6 (t) = 0 

and 

u dl (t) = 1 w rf2 (r) = -aco 2 sincor 

u d 3 ( t ) = 0 M d4 (0 = 0 

The initial conditions for the states are 

^rfi(O) — 1 x d2 (0) = am 
-WO) = 2 x rf4 (0) = 0 

W°) = 3 x d6 (0) = 0 



Choosing the six coincident closed-loop poles at -2, that is, 

Pi = Pi = Pi = Pa = Pi = P() = - 2 
we get the feedback matrix coefficients as 

k l = k 6 = 2 and k 2 = k 3 = k 4 = k 5 = 4 

Choosing a - 1 and CO = Jt, we get the simulation results. The results show the 
tracking errors for chained-form states and inputs, and for actual states and inputs. 
Once the tracking errors go to zero, the actual control inputs as obtained from the 
chained-form variables and inputs are the same as the computed desired inputs. The 
desired inputs are computed from actual system variables from Equation 4.5. Since 
the control design is based on the linearization of the system, the controller will 
make the controlled system locally asymptotically stable. The simulation results are 
presented in Figure 4.1 through 4.12. 
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FIGURE 4.1 The result of approximate linearization: tracking errors in chained-form vari- 
ables vs. time (s). 




FIGURE 4.2 The result of approximate linearization: tracking errors (m/s) in chained-form 
inputs vs. time (s). 
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FIGURE 4.3 The result of approximate linearization: actual (— ) and desired ( -) chained- 
form variables vs. time (s). 




FIGURE 4.4 The result of approximate linearization: actual (— ) and desired ( -) chained- 
form inputs vs. time (s). 
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FIGURE 4.5 The result of approximate linearization: tracking errors (m) in variables x, y, 
and z vs. time ( 5 ). 




FIGURE 4.6 The result of approximate linearization: tracking errors (rad) in variables \|/, 
0, and tp vs. time ( 5 ). 
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FIGURE 4.8 The result of approximate linearization: actual (— ) and desired ( -) original 
variables \|/, 0. and tp, vs. time (s). 
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FIGURE 4.11 



FIGURE 4.12 




0123456789 10 



The result of approximate linearization: v 3 (rad/s) vs. time (s). 




The result of approximate linearization: v 4 (rad/s) vs. time (s). 
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4.3.2 MATLAB® Program Code for the Approximate Linearization 



% Approximate Linearization for kinematic model, 
code % 



Main 



t= [0 : 0 . 01 : 10] ' ; a=l ; w=pi ; xd=t; xddot = l; 

yddot=a*w*cos (w*t) ; y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t) ; 
den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t,x] =ode4 5 ( 'F' , [t] , [-2,1,0]); 

[t , y] =ode45 ( 'FI' , [t] , [-l,a*w, 0] ) ; 
xe=-x( : , 1) +xd; ye=-y ( : , 1) +yd; 

theta=atan (y ( : , 2 ) . /x ( : , 2 ) ) ; thetad=atan (a*w*cos (w*t ) ) ; th 
etae= the tad- theta ; 

x2= (x ( : , 2) . *y ( : , 3) -x ( : , 3) . *y ( : , 2) ) ./ 

(x ( : , 1) . *x ( : , 1) . *x ( : , 1) ) ; 

phi=atan (cos (theta) . *cos (theta) . *cos (theta) . *x2 ) ; 
phid=atan ( (y2ddot . *xddot ) ./den) ; phie=phid-phi ; 

5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooooo 



% Matlab Functions 'F' and 'Fl'% 



5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 2 ' 2 ' 5 ' 5 ' 9 ' 5 ' 2 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooooo 



function xp = F(t,x) 
a=l ; w=pi ; 
xp = zeros (3,1) ; 
xp ( 1 ) =x ( 2 ) ; 
xp (2) =x (3 ) ; 

xp (3) = - 15*x ( 3 ) -75*x(2) - 125*x ( 1 ) +75 + 125* t ; 

function yp = Fl(t,y) 

a= 1 ; w=pi ; 

yp = zeros (3,1) ; 

yp ( 1 ) =y ( 2 ) ; 

yp ( 2 ) =y ( 3 ) ; 

yp(3)=-15*y(3) -75*y(2) -125*y (1) +75*a*w*cos (w*t ) - 
15*a*w*w*sin (w*t ) - a*w*w*w*cos (w*t ) +125*a*sin (w*t ) ; 



% Computation of tracking errors in chained form 
variables % 



%%% Original Syatem %%% 

A= [000000 ,-000000 ,-010000 ; 0000 
00100 ,- 000000 ] ; 

B= [1000 ;0100 ; 0000 ;0010; 0000 ; 0 
% %% Pole locations %%% 



sl=s2=s3=s4=s5=s6=-2; wn=2 ; qsi=l; 



0 0 ; 0 
0 0 1 ]; 
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%%% Feedback system %%%% 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

% %% response for the chained form error variables %%% 
p i = 2 2 / 7 ; a= 1 ; w=pi ; t= [0 : 0 . 01 : 9 . 999] ; 
xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %%%on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %% %off trajectory initially 
xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel = -K (1,1) *xef (1) ; ue2 = -K(2, 2) *xef (2) -K(2, 3) *xef (3) ; 
ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; ue4=-K (4 , 6 ) *xef (6 ) ; 
ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 

Figure (1); 

plot (t , x ( : , [1 , 3 , 5] ) ) , grid, title('Error in chained form 
states 1,3, and 5 :xel (t) , xe3 (t) , xe5 (t) ' ) ; 
hold on; 

plot (t , x ( : , [2 , 4 , 6] ) ) , grid, title ('Error in chained form 
states 2, 4, and 6 :xe2 (t) , xe4 (t) , xe6 (t) ' ) ; 
hold off; 



Computation of tracking errors in chained form inputs 



A= [000000 ,-000000 ,-010000 ; 000000 ; 0 
0 0 1 0 0 ; 0 0 0 0 0 0 ]; 

B= [1 0 0 0;0 1 0 0 ;0 0 0 0;0 0 1 0; 0000; 0001]; 
% %% Pole locations %%% 



Sl=s2=s3=s4=s5=s6=-2; wn=2 ; qsi=l; 

%%% Feedback system %%%% 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi ; t=[0:0.01:9.999]; 
xe0= [0 ,- a*w; 0 ,- 0 ,- 1 ,- 0] ,- %%%on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %% %off trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel = -K (1,1) *xef (1) ; ue2 = -K(2, 2) *xef (2) -K(2, 3) *xef (3) ; 
ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; ue4=-K (4 , 6 ) *xef (6 ) ; 
ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ,- xa=x' ; 

% %% response for the error in chained form inputs %%% 

url=-K (1 , 1) *xa (1 , : ) ; 

ur2 = -K (2,2) *xa(2, : ) -K (2 , 3 ) *xa(3, : ) ; 

ur3 = -K (3,4) *xa(4, : ) -K{3 , 5) *xa(5, : ) ; 

ur4 = -K (4 , 6 ) *xa (6 , : ) ; ur= [url ;ur2 ;ur3 ,-ur4] ; 
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Figure (2) ; 

Plot (t,ur), grid, title ('Error in inputs for chained 
form vs time ' ) ; 

plot (t,ur (1, : ) , 'g' ) ,grid; hold on; 
plot ( t , ur (2 , : ) , ' c ' ) , 

plot (t , ur (3 , : ) , ' b' ) ; plot (t , ur (4 , : ) , ' r ' ) ; hold off 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooo 

% Actual and desired chained form variables % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

%%% Feedback system %%%% 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi ; t= [0 : 0 . 01 : 9 . 999] ; 
xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %%%on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %% %off trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel = -K (1,1) *xef (1) ; ue2 = -K(2, 2) *xef (2) -K(2, 3) *xef (3) ; 
ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; ue4=-K (4 , 6 ) *xef (6 ) ; 
ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 

%%% response for the chained form actual and desired 
variables %%% 

xdl=t ; xd2=a*w*cos (w*t) ; xd3=a*sin (w*t) ; 
xd4=0*ones (1, 1000) ; xd5=l*ones (1, 1000) ; 
xd6 = 0*ones (1, 1000) ; 
xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 
xact=x' +xd ; 

Figure (3) ; 

plot (t , xd) ; grid, title (' Chained form desired states vs 
time' ) ; hold on; 

plot (t , xact ,'--') ; grid, title (' Chained form actual 
states vs time,'); hold off; 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooooooooo 



% Computation of actual and desired chained form inputs % 



o, o, o, o, o, 
o o o o o 


g, 

o 


o, o, 
o o 


g, g, 
o o 


g, g, 
o o 


0,0,0, 
o o o 


o, 

o 


0,0,0, 
o o o 


2-2-2-2-2-2-2-2-2-2-2-2- 

oooooooooooo 










A= [0 


0 


0 


0 


0 


0; 


0 


0 


0000 ;01000 


0; 


: 0 0 0 0 


0 


0 ; 0 


0 0 1 




0 


0; 


0 


0 0 




0 0 


0] ; 










B= [1 


0 


0 


0 


; 0 


1 


0 


0 


; 0 0 0 0 ; 0 0 1 0; 


0 


0 0 0 ; 0 


0 


0 1] ; 



% %% Pole locations %%% 

Sl=s2=s3=s4=s5=s6=-2; wn=2 ; qsi=l; 

%%% Feedback system %%%% 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi ; t= [0:0.01:9.999]; 
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xeO= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %%%on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %% %off trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1,1) *xef (1) ; ue2=-K(2, 2) *xef (2) -K(2, 3) *xef (3) ; 
ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; ue4=-K (4 , 6 ) *xef (6 ) ; 
ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; xa=x' ; 

%%% response for the chained form actual and desired 
inputs%%% 

url = -K (1 , 1) *xa (1 , : ) ; 

ur2 = -K (2,2) *xa(2, : ) -K (2 , 3 ) *xa(3, : ) ; 

ur3 = -K (3,4) *xa(4, :) -K(3,5) *xa(5, : ) ; 

ur4 = -K(4, 6) *xa (6, : ) ; 

ur= [url ;ur2 ;ur3 ;ur4] ; 

udl=l ; udln=udl*ones (1, 1000) ; 

ud2=-a*w^2*sin (w*t) ; ud2n=ud2' ; 

ud3=0*ones (1, 1000) ; ud4=0*ones (1, 1000) ; 

ud= [udln;ud2n,-ud3 ;ud4] ; 

u=ud+ur ; 

Figure (4); 

plot(t,ud), grid, title (' Chained form desired and actual 
inputs vs time, ' ) ; hold on; grid on 
plot (t , u, ' - - ' ) ; hold off 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooooo 

% Computation of tracking errors in variables x ,y , z % 

5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 2 ' 5 ' 5 ' 2 ' 2 ' 5 ' 5 ' 5 ' 9 ' 5 - 

ooooooooooooooooooooooooooooo 

%Feedback system 

K= [2 0 0 0 0 0 ; 044000; 000440; 00000 
2] ; %roots are ( -2 ) 

Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi;t=[0:0.01:9.999]; 

xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %of f trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1 , 1) *xef (1) ; 

ue2=-K (2,2) *xef (2) -K(2, 3) *xef (3) ; 

ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; 

ue4=-K(4, 6) *xef (6) ; 

ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 
xdl=t ; xd2=a*w*cos (w*t) ; xd3=a*sin(w*t); 
xd4 = 0*ones (1,10 0 0) ; xd5 = l*ones (1,10 0 0) ; 
xd6 = 0*ones (1, 1000) ; 
xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 
xact=x' +xd ; 
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^response for the errors in actual system variables? 



X=xact (1 , 
Y=xact (3 , 
Z=xact (5 , 



Wl= [ X ; Y ; Z ] ; 

Xd=xd ( 1 , : ) ; Yd=xd ( 3 , : ) ; Zd=xd ( 5 , : ) ; 

Wdl= [Xd ; Yd ; Zd] ; 

Wel=Wl -Wdl ; 

Figure (5) ; 

plot (t, Wei), grid, title ('Error in original state 
variables, x,y,z '); grid on 



Computation of tracking errors in variables \|f, 9, <p 



%Feedback system 

K= [2 00000 ; 044000; 000440; 00000 2]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi;t=[0:0.01:9.999]; 

xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %of f trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1 , 1) *xef (1) ; 

ue2=-K (2,2) *xef (2) -K(2, 3) *xef (3) ; 

ue3=-K (3,4) *xef (4) -K(3, 5) *xef (5) ; 

ue4=-K(4, 6) *xef (6) ; 

ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 
xdl=t ; xd2=a*w*cos (w*t) ; xd3=a*sin (w*t) ; 
xd4=0*ones (1, 1000) ; xd5=l*ones (1, 1000) ; 
xd6 = 0*ones (1,10 0 0) ; 
xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 
xact=x' +xd ; 

%%% response for the errors in actual system variables 

0 , 0 , 0 , 
o o o 

X=xact (1 , : ) ; 

Y=xact (3 , : ) ; 
z=xact (5 , : ) ; 

Wl= [ X ; Y ; Z ] ; 

PSI=atan (xact (2 , : ) ) ; 
theta=-atan (xact (4 , : ) . /sec (PS I ) ) ; 
phi=atan (xact (6, : ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 

W2= [PS I ; theta ; phi] ; 

PSId=atan (xd (2 , : ) ) ; 
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thetad=-atan (xd (4, : ) . / sec (PSId) ) ; 
phid=atan (xd ( 6 , : ) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) 
Wd2= [PSId; thetad;phid] ; 

We2=W2-Wd2 ; 

Figure (6); 

plot (t , We2 ), grid; title ('Error in original state 
variables, (psie, thetae,phie) ' ) ; 



Actual and desired original variables x, y, z 



K= [2 00000 ; 044000; 000440; 00000 2] 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi ; t= [0 : 0 . 01 : 9 . 999] ; 

xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %of f trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1 , 1) *xef (1) ; 

ue2=-K (2,2) *xef (2) -K(2, 3) *xef (3) ; 

ue3=-K (3,4) *xef (4) -K(3, 5) *xef (5) ; 

ue4=-K(4, 6) *xef (6) ; 

ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 
xdl=t ; 

xd2=a*w*cos (w*t) ; 

xd3=a*sin (w*t) ; 

xd4 = 0*ones (1, 1000) ; 

xd5 = l*ones (1, 1000) ; 

xd6 = 0*ones (1, 1000) ; 

xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 

xact=x' +xd ; 

x=xact (1 , : ) ; 

Y=xact (3 , : ) ; 

Z=xact (5 , : ) ; 

Wl= [ X ; Y ; Z ] ; 

Xd=xd ( 1 , : ) ; 

Yd=xd (3 , : ) ; 

Zd=xd (5 , : ) ; 

Wdl= [Xd ; Yd ; Zd] ; 

Figure (7); 

plot (t,Wl) , grid, title ( 'Original system actual state 
variables, (x,y, z) ') ;hold on; 

plot (t , Wdl ,'--') ; title ( 'Original system actual state 
variables , (xd, yd, zd) '); hold off 
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9 ' 5 ' 9 ' 5 ' 2 ' 5 ' 9 ' 5 ' 2 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 2 ' 2 ' 5 - 

ooooooooooooooooooooooooo 

% Actual and desired original variables \|f, 0, <p % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi;t=[0:0.01:9.999]; 

xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %of f trajectory initially 

xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1 , 1) *xef (1) ; 

ue2=-K (2,2) *xef (2) -K(2, 3) *xef (3) ; 

ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; 

ue4=-K(4, 6) *xef (6) ; 

ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; 
xdl=t ; 

xd2=a*w*cos (w*t) ; 

xd3=a*sin (w*t) ; 

xd4 = 0*ones (1, 1000) ; 

xd5 = l*ones (1, 1000) ; 

xd6 = 0*ones (1, 1000) ; 

xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 

xact=x' +xd ; 

x=xact (1 , : ) ; 

Y=xact (3 , : ) ; 

Z=xact (5, : ) ; 

Wl= [ X ; Y ; Z ] ; 

Xd=xd ( 1 , : ) ; 

Yd=xd (3 , : ) ; 

Zd=xd (5 , : ) ; 

Wdl= [Xd ; Yd ; Zd] ; 

PSI=atan (xact (2 , : ) ) ; 
theta=-atan (xact (4 , : ) . /sec (PS I ) ) ; 
phi=atan (xact (6, :) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 

W2= [PS I ; theta ; phi] ; 

PSId=atan (xd (2 , : ) ) ; 
thetad=-atan (xd (4 , : ) . / sec (PSId) ) ; 
phid=atan (xd ( 6 , : ) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

Wd2= [PSId; thetad;phid] ; 

We2=W2-Wd2 ; 

Figure (8); 

plot (t,W2) , grid, title ( 'Original system actual state 
variables, (psi, theta, phi) ');hold on; 
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plot (t , Wd2 ; title ( 'Original system actual state va 
riables, ( (psid, thetad,phid) ' ) ;hold off; 



9 ' 9 ' 2 ' 5 ' 9 ' 5 ' 9 ' 5 ' 9 ' 5 ' 9 ' 5 ' 2 ' 9 ' 9 ' 2 ' 9 ' 5 ' 9 ' 5 ' 9 ' 5 - 

oooooooooooooooooooooo 

% Computation of actual input v 2 % 

9 ' 5 ' 5 ' 5 ' 2 ' 9 ' 9 ' 2 ' 9 ' 5 ' 9 ' 5 ' 5 ' 5 ' 9 ' 5 ' 9 ' 5 ' 9 ' 5 ' 2 ' 5 - 

oooooooooooooooooooooo 



A= [0 0 0 0 0 0 ,-0 0 0 0 0 0 ,-0 1 0 0 0 0 ; 000000 ; 0 
0 0 1 0 0; 0 0 0 0 0 0]; 

B= [1 0 0 0;0 1 0 0 ;0 0 0 0;0 0 1 0; 0000; 0001]; 
% %% Pole locations %%% 

Sl=s2=s3=s4=s5=s6=-2; wn=2 ; qsi=l; 

%%% Feedback system %%%% 

K= [2 00000; 044000; 000440; 000002]; 
Acl = A - B*K; C=zeros (6, 6) ; D=zeros ( 6 , 4 ) ; 
sys=ss (Acl , B , C, D) ; 

p i = 2 2 / 7 ; a= 1 ; w=pi ; t= [0 : 0 . 01 : 9 . 999] ; 
xe0= [0 ; a*w; 0 ; 0 ; 1 ; 0] ; %%%on trajectory initially 

xe0= [1 ; a*w; 2 ; 0 ; -3 ; -2] ; %% %off trajectory initially 
xef = [0 ; 0 ; 0 ; 0 ; 0 ; 0] ; 

uel=-K (1,1) *xef (1) ; ue2=-K (2,2) *xef (2) -K(2, 3) *xef (3) ; 
ue3=-K (3,4) *xef (4) -K (3 , 5) *xef (5) ; ue4=-K (4 , 6 ) *xef (6 ) ; 
ue= [uel ;ue2 ;ue3 ;ue4] ; ue=ue*ones (1,1000) ; 

[y, t,x] =lsim(sys,ue, t,xe0) ; xa=x' ; 
xdl=t ; 

xd2=a*w*cos (w*t) ; 

xd3=a*sin (w*t) ; 

xd4 = 0*ones (1, 1000) ; 

xd5=l*ones (1,1000) ; 

xd6 = 0*ones (1,10 0 0) ; 

xd= [xdl ' ; xd2 ' ; xd3 ' ; xd4 ; xd5 ; xd6 ] ; 

xact=x' +xd ; 

url=-K(l, 1) *xa (1, : ) ; 

ur2=-K (2,2) *xa(2, :) -K(2,3) *xa(3, : ) ; 

ur3=-K (3,4) *xa (4, : ) -K(3, 5) *xa (5, : ) ; 

ur4=-K(4, 6) *xa (6, : ) ; 

ur= [url ;ur2 ;ur3 ;ur4] ; 

udl=l ; 

udln=udl*ones (1, 1000) ; 
ud2=-a*w^2*sin (w*t) ; 
ud2n=ud2 ' ; 
ud3 = 0*ones (1,10 0 0) ; 
ud4 = 0*ones (1, 1000) ; 
ud= [udln;ud2n,-ud3 ;ud4] ; 
u=ud+ur ; 

PSI=atan (xact (2 , : ) ) ; 
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theta=-atan (xact (4 , : ) . /sec (PS I ) ) ; 
phi=atan (xact (6, : ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
vl=u (1 , : ) . / cos (PS I ) . *cos (theta) ; 

Figure (9); 

plot (t,vl) , grid, title ( 'original system inputl (vl'); 

2 - 9 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Computation of input v 2 % 

5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 - 

ooooooooooooooooooooooooooo 

rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 

v3=rll . * (-r23 . *u(2, : ) -r33 . *u(3, : ) ) ; 
v4 = rll . * (r22 . *u (2, : ) +r32 . *u(3, : ) ) ; 
v2=rlln . * (R1 . *u (4, : ) -rl2 . *v3-rl3 . *v4) ; 

Figure (10) ; 

plot (t,v2) , grid, title ( 'original system input2 (v2'); 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Computation of input v3 % 

5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooo 

rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / (1+rll) ; 

v3=rll . * (-r23 . *u(2, : ) -r33 . *u(3, : ) ) ; 

Figure (11) ; 

plot (t,v3 ), grid, title ( 'original system input3 (v3 ' ) ; 

2 - 9 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 9 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Computation of input v 4 % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooooo 

rll=cos (PS I ) . *cos (theta) ; 
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r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 

v4=rll . * (r22 . *u (2, : ) +r32 . *u(3, : ) ) ; 

Figure (12) ; 

plot (t,v4) , grid, title ( 'original system input4 (v4)'); 



4.4 CONTROL USING EXACT FEEDBACK LINEARIZATION 
VIA STATE AND INPUT TRANSFORMATIONS 

In this section the use of nonlinear feedback design is used for the global stabili- 
zation of the tracking error associated with the trajectory to zero. For nonlinear 
systems two types of exact linearization methods are generally used. One is the 
full-state feedback transformation of the differential equations of the system into the 
linear system. Another is the input-output linearization, which results in the input- 
output differential map being linear. Both the feedback problems can be solved using 
either the static or the dynamic feedback. 

For the nonholonomic driftless system q = G(q)v the full-state lineariza- 
tion of the system cannot be achieved by using a smooth static (time-invariant) 
state feedback. The reason for this is in fact the controllability condition given by 
rank[g 1 ,g 2 ,g 3 ,g 4 ,[g l ,g 3 ][g 1 ,g i ]] = 6, which means that the distribution generated by the 
vector fields , g 2 , g 3 , g 4 is not involutive, which violates the necessary condition for 
full-static-state feedback linearization [13]. Thus, for exact linearization of the sys- 
tem, the method of dynamic state feedback is used. 

For the above nonlinear system, dynamic feedback linearization consists of find- 
ing a dynamic feedback compensator of the form 



t, = a(q,C,) + b(q,Q r 

u = c(q,Q + d{q,C,)r 



(4.20) 



The state vector 'C is the compensator state whose dimensions depend upon the num- 
ber of integrators added on the input channels. The vector r is the auxiliary input 
vector, which is the new input to the integrators added. 

The starting point of the dynamic extension for our problem is to define an m 
dimensional output vector z = Mq), m being 4 in this case. A certain desired behavior 
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is assigned to this output vector. The output vector is then successively differentiated 
until each and every input in the system appears and the invertible map (or matrix) 
is nonsingular. During the successive differentiations of the output vector it becomes 
necessary to add the chain of integrators on the inputs so as to avoid their direct dif- 
ferentiation. The number of integrators results in the compensator state vector The 
input to these integrators becomes the new auxiliary input r. The process continues 
and terminates after a finite number of differentiations if the system is invertible 
from the chosen output vector z. If the sum of the orders of the output differentiations 
is equal to the sum of the order of the original system (n) and the dimensions of the 
compensator, then the full-state linearization is achieved in the sense that no internal 
dynamics are left in the system. The process also results in the decoupling of the 
output vector from the new auxiliary input. 

If at some point of differentiation of output in the algorithm the decoupling matrix 
of the system is nonsingular without the addition of any compensator state, the pro- 
cess results in the input-output linearization of the system. The static feedback law 
of the form 



u = a{q) + b(q)r 



( 4 . 21 ) 



is used to linearize the system. 



4.4.1 Control Using Exact Feedback Linearization via Static Feedback 

For our system let the output vector in the chained form be defined as 



( 1 



z = 



x 3 

x 5 



V 



*6 



J 



The derivative of the output is given as 



*1 1 




' 1 


0 


0 


0 " 


f \ 

u x 


i 3 




*2 


0 


0 


0 


u 2 


*5 




a 4 


0 


0 


0 


u 3 






l 0 


0 


0 




Ua 

v 4 7 



( 4 . 22 ) 



or 



Z = H{q)u 



( 4 . 23 ) 
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The inputs u 2 and m 3 do not appear after the differentiation, and also the decou- 
pling matrix H(q) is singular and not invertible. Hence, the static feedback cannot be 
applied and the system cannot be linearized by input-output linearization. The same 
result follows if the above procedure is repeated by choosing the actual state variable 
as the output vector. 



4.4.2 Control Using Exact Feedback Linearization via Dynamic Feedback 

Since the static feedback fails to solve the problem, we will be making use of dynamic 
feedback extension. For the linearization via dynamic feedback let us again define 
the linearizing output vector for the chained form as 



( *1 ) 



z = 



*3 

*5 



V 



*6 



J 



Differentiating with respect to time we get 



( 1 



1 



z = 



x 3 
*5 



*2 

a 4 



Xf. 

V 6 J 



0 


0 


0 " 


f \ 

u x 


0 


0 


0 


u 2 


0 


0 


0 


u 3 


0 


0 


1 , 


U 4 



(4.24) 



(4.25) 



In order for the algorithm to proceed, we need to add two integrators whose states 
are denoted by and i^ 2 on the inputs u l and m 4 , respectively, so their differentiation 
in the next step is avoided. Thus, 



Mi — M 4 — C , 2 

Cl = U 1 C 2 = M 4 



(4.26) 



where u[ and u 4 are the new auxiliary inputs on the system. Substituting the above 
values and differentiating the output vector again we obtain 



z = 



u A 

X 2 U\ + x 2 u x 
x A u x + x 4 ii l 
u 4 



Cl 

ijCi + ^Ci 

x 4 u 1 + x 4 ^ 

c 2 
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or 





( > ^ 
u 2 




' 1 


0 


0 


°1 


f 


/ A 
u x 




Cl«2 + ^2«1 




x 2 


Cl 


0 


0 




u 2 


z — 


^m 3 + x 4 u[ 




x 4 


0 


Ci 


0 




« 3 




u'. 

\ J 




l 0 


0 


0 




V 


u\ 



In the above equations all the inputs appear in a nonsingular way, that is, the 
decoupling matrix is nonsingular. The value of the determinant of the matrix is 
Q. Thus, the algorithm terminates after two differentiations. The matrix is non- 
singular only as long as ^ 0 or 0. Here the order of the compensator is two 
(. b = 2) and the number of states in the system is six (n = 6). The sum of the order 
of differentiations that is eight is equal to n + b. Thus, full-state linearization is 
achieved. 

Let Equation 4.27 be rewritten as z = r, where r is the auxiliary reference input. 
Therefore, we have the following decoupled chains of integrators 



z'i = r x 



z 2 = r 2 



z 3 = r 3 



(4.28) 



z 4 = r 4 



The resulting nonlinear dynamic feedback controller is 

u i = Ci 

u 2 = (r 2 - x 2 r l % l 
u i = (r 3 -x 4 r 1 % 1 

(4.29) 

M 4 — C2 

Ci = "i = h 
C 2 = *4 = r 4 



Assuming the system follows a smooth desired reference trajectory in chained- 
form coordinates as z d (t) = (x dl (t),x d3 (t),x d5 (t),x d6 (t)), the exponentially stabilizing 
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feedback control law for the linear and decoupled system about this desired trajec- 
tory is given as 

n = z di (t)+k vi (z di (t)-z i (t))+k pi (z di (.t)-z i (t)y, i = 1,2..., 4 (4.30) 

where k vi and k pi are the proportional and derivative (PD) gains and are chosen such 
that they are positive and the characteristic polynomials 

s 2 + k vi s+k pi ; i = 1,2. ..,4 (4.31) 



are Hurwitz. The desired values for the variables z d and z d are obtained from 
Equations 4.25 and 4.27. For the simulation the other state variables can be expressed 
in terms of the desired output trajectory at the initial time t = t 0 : 



Xl(t 0 ) = Z dl (t 0 ) = X d (t 0 ) 

x 2^o) = Z d 2(t 0 )/z d i(t 0 ) = yd( f 0 )!x d {t 0 ) 

X i(?o) = Zdltto) — y d (to) 

x 4 (h) = Zdltto VZdlito) = Z d (t 0 V x di^o) (4.32) 

x d 5 ^o) = Zd 3 (^ 0 ) = Z d ifo) 
x d6^o) = Z d 4^o) 

Cl( ? o) = Z d l(tg) 

C 2 ( ? 0 ) = Z d 4(t 0 ) 



From this initialization, the output trajectory is reproducible. Any other ini- 
tialization gives tracking error, which exponentially goes to zero with time. The 
same results will follow if the dynamic extension is applied to the original kine- 
matic equations. 



4.4.3 Simulation of the Controller 

For simulation, here again the same desired trajectory is used as was used in the 
linearized case. The trajectory chosen is 

Z d] (t) = x d (t) = t z d2 (t ) = y d (t) = a sincof 
Z d3 (0 = z d (0 = 1 Z d4 (t) = x (l6 (t) = 0 
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which gives the desired values for the chained-form states and inputs as follows: 
x d\( t ) = r x d 2 (t) = acocoscot x d3 (t) = asina>t 
x d 4 (t) = 0 x ds (t) = 1 x d6 (t) = 0 



and 



M rf2 (0 = -«w 2 sincot u d3 (t) = 0 

The initial conditions for the states are 

*di (0) = 0 x d 2 (0) = flCO x d3 (0) = 0 
x d 4 (0) = 0 x d5 (0) = 1 x rf6 (0) = 0 

= MjiCO) = i t^ d 2 (0) = u d 4 (0) = o 

Choosing again the six coincident closed-loop poles at -2, that is, p, = -2; 
i = we get the PD gains as k vi = 4 and k pi = 4. Choosing a — 1 and CO = Jt 

we get the simulation results. The results show the tracking errors for chained- 
form states and inputs, and for actual states and inputs. Once the tracking 
errors go to zero, the actual inputs (control inputs) are the same as the desired 
inputs. The simulation results are presented in Figures 4.13 through 4.24. 




FIGURE 4.13 The result of dynamic feedback: tracking errors in chained-form variables 
vs. time (s). 
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0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 



FIGURE 4.16 The result of dynamic feedback: actual (— ) and desired ( -) chained-form 
inputs vs. time (s). 




FIGURE 4.17 The result of dynamic feedback: tracking errors (m) in variables x, y, and z 
vs. time (s). 
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FIGURE 4.18 The result of dynamic feedback: tracking errors (rad) in variables \|/, 0, and 
tp vs. time (s). 




FIGURE 4.19 The result of dynamic feedback: actual (— ) and desired ( -) original variables 
x, y, z vs. time (.s) . 



© 2011 by Taylor and Francis Group, LLC 





FIGURE 4.20 The result of dynamic feedback: actual (— ) and desired ( -) original variables 
\| /. 0, and tp vs. time (s). 




FIGURE 4.21 The result of dynamic feedback: v, (m/s) vs. time (s). 
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FIGURE 4.24 The result of dynamic feedback: v 4 (rad/s) vs. time (s). 

4.4.4 MATLAB Program Code for Dynamic Extension 



Approximate linearization through Dynamic Extension 



t= [0:0. 01:10] ' ; a=l; w=pi; 
xd=t ; xddot=l ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddot 
+yddot . *yddot) ) . * (sqrt (xddot . *xddot+yddot . *yddot) ) ; 

[t,x] =ode4 5 ( 'FI' , [t] , [-2, 1] ) ; 

[t , y] =ode45 ( ' F2 ' , [t] , [a*w, 1] ) ; 

[t, z] =ode4 5 ( 'F3' , [t] , [0,1]); 
xe=-x ( : , 1) +xd ; 
ye=-y ( : , l) +yd; 
ze=-z ( : , 1) +zd; 
theta=atan (y ( : , 2 ) . /x ( : , 2 ) ) ; 
thetad=atan (a*w*cos (w*t) ) ; 
thetae= the tad- theta ; 

x2= (x(: ,2) . *y ( : , 3) -x ( : , 3) . *y ( : , 2) ) ./ 

(x ( : , 1) . *x ( : , 1) . *x ( : , 1) ) ; 

phi=atan (cos (theta) . *cos (theta) . *cos (theta) . *x2 ) ; 
phid=atan ( (y2ddot . *xddot ) . / den) ; 
phie=phid-phi ; 

2 - 9 - 2 - 2 - 2 - 5 - 2 - 2 - 2 - 9 - 2 - 9 - 9 - 5 - 9 - 9 - 9 - 9 - 2 - 2 - 2 - 5 - 2 - 2 - 9 - 2 - 2 - 2 - 

oooooooooooooooooooooooooooo 
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% Matlab Functions 'FI', ' F2 ' , ' F3 ' and ' F4 ' % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooooo 



function xp = F(t,x) %%% function 'FI' 
a=l ; w=pi ; 
xp = zeros (2,1) ; 
xp (1) =x (2) ; 

xp (2 ) =-4*x ( 1 ) -4*x(2)+4+4*t; 

function yp = F (t,y) %%% function ' F2 ' 

a= 1 ; w=pi ; 

yp = zeros (2,1) ; 

yp ( 1 ) =y ( 2 ) ; 

yp(2)=-4*y(2) -4*y (1) -a*w*sin (w*t ) +4*a*w*cos (w*t) +4*a*sin 
(w*t) ; 

function zp = F4(t,z) %%% function 'F3' 

a= 1 ; w=pi ; 
zp = zeros (2,1) ; 
zp (1) =z (2) ; 

zp (2) = -4*z (2) -4*z (1) +4 ; 

function sp = F4(t,s) %%% function ' F4 ' 

a=l ; w=pi ; 

sp = zeros (2,1) ; 

sp (1) =S (2) ; 

sp (2) =-4*S (2) -4*S (1) ; 



Tracking errors in chained form variables 



t= [0:0. 001:9. 999] ' ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd= 1 ; sd=0; 
xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 



response for the 


chained form error variables 


[ t , x] =ode45 ( 'FI' , 


• [t] , 


[1,1] ) ; 


[t , y] =ode4 5 ( ' F2 ' , 


• [t] , 


[2,1] ) ; 


[t, z] =ode45 ( 'F3' , 


• [t] , 


[-3,0] ) ; 


[t, s] =ode4 5 ( 'F4' , 


• [t] , 


[-2,0] ) ; 
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x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x( : , 2) ; 
xe=x ( : , 1) -xd; 
ye=y ( : , l) -yd; 
ze=z ( : , 1) -zd' ; 
se=s ( : , 1) -sd; 



xe2=x2 -xd2 ; 
xe4=x4 -xd4 ' ; 

Figure (1) 

plot(t, xe,'b')/ grid, title ('Error in chained form 

states'); hold on 

plot (t, xe2, 'g' ) ; 

plot ( t , ye , ' r ' ) ; 

plot (t,xe4, ' c' ) ; 

plot (t, ze,'m') 

plot(t, se,'y'); hold off 



Computation of tracking errors in chained form inputs 



xd=t ; yd=a*sin (w*t) ; zd=l; t= [0 : 0 . 001 : 9 . 999] ' ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t,x] =ode45 ('FI', [t] ,[1,1]); 

[t , y] =ode4 5 ( ' F2 ' , [t] , [2 , 1] ) ; 

[t, z] =ode45 ( 'F3' , [t] , [-3,0]); 

% %% error in chained form inputs %%% 
rl=4+4*t-4*x(:,2)-4*x(:,l) ; 

r2=-4*y( : , 2) -4*y ( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t ) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 

Ul=x ( : , 2) ; 



zetal=ul ; 

u2= (r2-x2 . *rl) ./zetal; 
u3= (r3-x4 . *rl) . /zetal ; 
u4 = s ( : , 1) ; 

udl = l*ones (1, 10000) ; 

ule=ul -udl ' ; 

ud2=-a*w^2*sin (w*t) ; 

u2e=u2 -ud2 ; 

ud3 = 0*ones (1, 10000) ; 

u3e=u3 -ud3 ' ; 

ud4 = 0*ones (1, 10000) ; 

u4e=u4 -ud4 ' ; 
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Figure (2) 

plot (t,ule, 'b' ) , grid, title ('Error in chained inputs'); 
hold on 

plot (t, u2e, 'g' ) ; 

plot (t,u3e, 'r'); 

plot (t,u4e, ' c' ) ; hold off 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

% Actual and desired chained form variables % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

t= [0:0. 001:9. 999] ' ; a=l; w=pi ; 

xd=t ; yd=a*sin (w*t) ; 

zd= 1 ; sd=0; 

xd2=a*w*cos (w*t) ; 

xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

%%% response for the chained form actual variables %%% 
[t,x] =ode45 ('FI', [t] ,[1,1]); 

[t , y] =ode4 5 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode45 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode4 5 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

Figure (3) 

plot(t, xd,'b'), grid, title (' Chained form desired and 

actual variables ');hold on 

plot (t, x ( : , 1) ,'--') ; 

plot (t, xd2, 'g' ) ; 

plot (t , x2 , ' g - - ' ) ; 

plot (t , yd, ' r ' ) ; 

plot (t , y ( : , 1) , ' r- - ' ) ; 

plot (t,xd4, ' c' ) ; 

plot (t , x4 , ' c - - ' ) ; 

plot (t , zd, ' m' ) ; 

plot (t, z(:,l),'m '); 

plot (t , sd, ' y' ) 

plot(t, s ( : , 1) , 'y-- ' ) ; hold off 



5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 9 ' 2 ' 5 ' 2 ' 9 ' 2 ' 5 ' 5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 2 ' 5 ' 2 ' 2 - 

ooooooooooooooooooooooooooooooo 



% Computation of actual and desired chained form inputs % 
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5 ' 5 ' 9 ' 5 ' 2 ' 5 ' 2 ' 5 ' 2 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 - 

ooooooooooooooooooooooooooooooo 

t= [0:0. 001:9. 999] ' ; a=l; w=pi ; 

[t,x] =ode45 {'FI', [t] ,[1,1]); 

[t,y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y { : , 2) . /x( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

%%% response for the chained form actual and desired 
inputs%%% 

rl=4+4*t-4*x(:,2)-4*x(:,l) ; 

r2=-4*y( : , 2) -4*y ( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 
ul=x ( : , 2) ; 
zetal=ul ; 

u2= (r2-x2 . *rl) ./zetal; 
u3= (r3-x4 . *rl) ./zetal; 
u4 = s ( : , 1) ; 

udl = l*ones (1, 10000) ; 
ud2=-a*w^2*sin (w*t) ; 
ud3 = 0*ones (1, 10000) ; 
ud4 = 0*ones (1, 10000) ; 

Figure (4) 

plot(t, udl,'b'), grid, title ('Chained form desired and 

actual inputs'); hold on 

plot (t, ul, '--'); 

plot (t, ud2, 'g' ) ; 

plot (t , u2 , ' g- - ' ) ; 

plot (t , ud3 , ' r ' ) ; 

plot (t , u3 , ' r- - ' ) ; 

plot (t,ud4, ' c' ) ; 

plot(t, u4 , ' c - - ' ) ; hold off 

2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 2 ' 2 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 9 - 

ooooooooooooooooooooooooooooo 

% Computation of tracking errors in variables x , y , z % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooooooo 

t= [0:0. 001:9. 999] ' ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 
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[t , x] =ode4 5 ( ' FI ' , [t] , 
[t , y] =ode45 ( ' F2 ' , [t] , 
[t, z] =ode4 5 ( 'F3' , [t] , 
^response for the errors 



[ 1 , 1 ] ) ; 

[2,1] ) ; 

[-3,0] ) ; 

in actual system variables" 



xe=x ( 

ye=y ( 

ze=z ( 



, 1 ) 
, 1 ) 
, 1 ) 



xd; 
■yd; 
■ zd' 



Figure (5) 

plot(t, xe,'b'), grid, title (' Error in original state 

variables, x,y,z '); grid on 

plot (t, ye , ' g ' ) ; 

plot (t, ze, ' r' ) ; hold off 



Computation of tracking errors in variables \|f, 9, <p 



xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
sd=0 ; 

xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[ t , x] =ode45 ( 'FI' , [t] , [1,1]); 

[t , y] =ode4 5 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode45 ( 'F3' , [t] , [-3,0]); 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

%%% response for the errors in actual system variables 
(psi, theta, phi)%%% 

PSI=atan (x2 ) ; 

PSIe=PSI-PSId; 
theta=-atan (x4 . /sec (PSI ) ) ; 
thetae= theta- thetad; 

phi=acot (cot (theta) . /sin (PSI ) + tan (PSI ) . /sin (theta) ) ; 
phie=phi -phid ; 

Figure (6) 

plot(t, PSIe, ' b ' ) ; % , grid, title ('Error in original 
state variables, (psi, theta, phi) ' ) ;hold on 
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plot(t, thetae, ' g' ) 
plot(t, phie, ' r' ) ; hold off 



Actual and desired original variables x, y, z 



t= [0:0. 001:9. 999] ' ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t,x] =ode4 5 ('FI', [t] ,[1,1]); 

[t,y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x( : , 2) ; 

Figure (7) 

plot(t, xd, 'b'), grid, title ( 'Original system actual and 

desired state variables, (x, y, z) ' ;hold on 

plot (t, x ( : , 1) ; 

plot (t, yd, 'g' ) ; 

plot (t, y ( : , 1 ) , ' g - - ' ) ; 

plot (t , zd, ' r' ) ; 

plot (t , z ( : , 1) , ' r- - ' ) ; hold off 

5 ' 5 ' 5 ' 2 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 - 

ooooooooooooooooooooooooo 

% Actual and desired original variables \|/, 0, cp % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 



xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
xddot =1 ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 
xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

[t,x] =ode45 ( 'FI' , [t] ,[1,1]); 

[t , y] =ode4 5 ( ' F2 ' , [t] , [2,1]); 
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[t,z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

PSI=atan (x2 ) ; 

PSIe=PSI-PSId; 
theta=-atan (x4 . /sec (PS I ) ) ; 
thetae=theta-thetad; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
phie=phi -phid ; 

Figure (8) 

plot(t, PSId, 'b ');%, grid, title (' Original system 

actual state variables, (psi, theta, phi) '); hold on 

plot (t,PSI, ' ) ; 

plot(t, thetad, 'g' ) 

plot (t , theta, ' g--'); 

plot (t , phid, ' r ' ) ; 

plot(t, phi,'r--'); hold off 



5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 2 ' 5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 - 

oooooooooooooooooooooo 

% Computation of actual input vl% 

2 - 9 - 9 - 2 - 2 - 9 - 2 - 9 - 9 - 2 - 2 - 2 - 9 ' 2 - 2 - 2 - 9 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooo 

xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[ t , x] =ode4 5 ( 'FI' , [t] ,[1,1]); 

[t , y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0]); 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PSI ) ) ; 

phi=acot (cot (theta) . /sin (PSI )+ tan (PSI ) . /sin (theta) ) ; 

%%% Actual control variable v 1 %%% 
rl=4+4*t-4*x(: ,2) -4*x( : ,1) ; 

r2=-4*y( : , 2) -4*y ( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t ) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 
ul=x ( : , 2) ; 
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zetal=ul ; 

u2= (r2-x2 . *rl) ./zetal; 
u3= (r3-x4 . *rl) ./zetal; 
u4 = s ( : , 1) ; 

vl=ul . / cos (PS I ) . *cos (theta) ; 

Figure (9) ; 

plot (t,vl) , grid, title ( 'original system inputl (vl'); 

5 ' 5 ' 5 ' 2 ' 2 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooo 

% Computation of input v 2 % 

2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 2 ' 5 ' 5 ' 5 - 

ooooooooooooooooooooooooooo 

xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot= -a*w*w*sin (w*t ) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t,x] =ode4 5 ('FI', [t] ,[1,1]); 

[t , y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PS I ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
rl=4+4*t-4*x(:,2)-4*x(:,l) ; 

r2=-4*y( : , 2) -4*y ( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t ) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 

Ul=x ( : , 2) ; 

zetal=ul ; 

u2= (r2-x2 . *rl) ./zetal; 
u3= (r3-x4 . *rl) ./zetal; 
u4 = s ( : , 1) ; 

%%% Actual control variable v 2 %%% 
vl=ul . / cos (PS I ) . *cos (theta) ; 
rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 



© 2011 by Taylor and Francis Group, LLC 




Control Design Using the Kinematic Model 



97 



Rl=l+R; 

rlln=l . / ( 1 + rll ) ; 
v3=rll . * ( -r23 . *u2-r33 . *u3) ; 
v4=rll . * (r22 . *u2+r32 . *u3) ; 
v2=rlln . * (R1 . *u4-rl2 . *v3-rl3 . *v4) ; 

Figure (10) ; 

plot (t,v2),grid, title ( 'original system input2 (v2'); 

5 ' 5 ' 5 ' 2 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooo 

% Computation of input v 3 % 

5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 - 

oooooooooooooooooooooooooo 

t= [0 : 0 . 001 : 9 . 999] ' ; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t,x] =ode45 ('FI', [t] ,[1,1]); 

[t,y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PS I ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
rl=4+4*t-4*x(:,2)-4*x(:,l) ; 

r2=-4*y( : , 2) -4*y ( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 
ul=x ( : , 2) ; 
zetal=ul ; 

u2= (r2-x2 . *rl) ./zetal; 
u3= (r3-x4 . *rl) ./zetal; 
u4 = s ( : , 1) ; 

%%% Actual control variable v 3 %%% 
rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / (1+rll) ; 

v3=rll . * ( -r23 . *u2-r33 . *u3) ; 
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Figure (11) ; 

plot (t,v3 ), grid, title ( 'original system input3 (v3 ' ) ; 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Computation of input v 4 % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooooo 

t= [0 : 0 . 001 : 9 . 999] ' ; 

[t,x] =ode4 5 ('FI', [t] ,[1,1]); 

[t , y] =ode45 ( ' F2 ' , [t] , [2,1]); 

[t, z] =ode4 5 ( 'F3' , [t] , [-3,0]); 

[t , s] =ode45 ( ' F4 ' , [t] , [-2,0] ) ; 
x2=y ( : , 2) . /x ( : , 2) ; 
x4 = z ( : , 2) . /x ( : , 2) ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PS I ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
rl=4+4*t-4*x(:,2)-4*x(:,l) ; 

r2=-4*y( : , 2) -4*y( : , 1) -a*w*w*sin(w*t) +4*a*w*cos (w*t) +4* a* 
sin (w*t) ; 

r3 = -4*z ( : , 2) -4*z ( : , 1) +4 ; 
ul=x ( : , 2) ; 
zetal=ul ; 

u2= (r2-x2 . *rl) ,/zetal; 
u3= (r3-x4 . *rl) ,/zetal; 
u4 = s ( : , 1) ; 

%%% Actual control variable v 4 %%% 
rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 

v4=rll . * (r22 . *u2+r32 . *u3) ; 

Figure (12) ; 

plot (t,v4) , grid, title (' original system input4 (v4'); 



4.5 POINT-TO-POINT STABILIZATION 

In the following section the problem of point-to-point stabilization is addressed. The sys- 
tem is supposed to reach a final desired configuration starting from an initial point, with- 
out the need to plan a trajectory. As stated earlier, point stabilization cannot be achieved 
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by a smooth time-invariant feedback. Only nonsmooth or time -varying feedback laws 
are of interest for the task. For our system we will adopt the latter approach. 

4.5.1 Control with Smooth Time-Varying Feedback 

The method of designing a stabilizing control law here is the one proposed in [7]. 
The control law presented there was for a two-input nonholonomic system. The con- 
troller here is an extension of the same. The statement of the problem is: given a 
nonlinear drift-free control system (Equation 3.11) 

q = g\(q) v \ + g 2 (q) v 2 + g^q) v 3, + gM) v * ( 4 - 33 ) 

we have to find a control law of the form v(q, t ) that makes the origin globally stable. 
In [7] the origin of the control system (Equation 4.33) is stabilized as represented in 
power form. Thus, before going for the control design, we will convert Equation 4.33 
into a power form. 



4.5.2 Power Form 

The method of converting Equation 4.33 to power form is presented in [24]. The 
transformation is done in two steps. In the first step the original system is converted 
into a three-chain, single-generator chained form, as described in Section 3.3. The 
chained form there obtained is given as 



i, = xj* = m, x 2 = x$ = u 2 x 4 = x§ = w 3 

X 6 = *4 =u 4 *3 = A 20 = X 2 lh *5 = *31 = X 4 U t 



(4.34) 



In the second step the chained-form system (Equation 4.34) is converted into 
power form. For the three-chain single-generator chained form the global transfor- 
mation to power form is given below [24]: 



yj = xf, 1 < j < 4 



k - 1 

Zjo = (-!)**£) + J(-l)- (k _ ri) M?f n x "o 2<j<4l<k< 



(4.35) 



S / S 4 1 S K s n , 



which gives the power form as 

.'7 »;:!<./< 4 

Z% = Jjiyi ) k Uj 2 < j < 4 1 < k < tij 



(4.36) 
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Here we should recall from Section 3.3 that x% = x ( j is identified as the top of the 
chains and 2 n ; = 2. Thus, we have n 2 = n 3 = 1 and n 4 = 0. Using Equation 4.34 
and taking the above values, the transformation Equation 4.35 becomes 



yi = xi 

yi = x 2 
y 3 = x 4 

y 4 = x 6 

Z i =Z20=-X 3 + X I X 2 
Z 2 = Z30=-X 5 +X 1 X 4 
Z 3 = Z$ 0 = X 6 



(4.37) 



and the corresponding power form is 



>i = «i 

y 2 = u 2 
y 3 = m 3 

y 4 = u 4 

z 20 = y\ u 2 

^30 = T 1 M 3 
240 = «4 



(4.38) 



4.5.3 Control Design with Smooth Time-Varying Feedback 

The smooth time-varying feedback control law for Equation 4.38 as adopted from 
[7] is given here as 



M i = — .Vi + G(p(z))(cos t — sin t) 



u 2 = —y 2 +c l z l cos t 



m 3 = ~y 3 +c 2 z 2 cos t 



(4.39) 



« 4 = ~y 4 
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with c h c 2 > 0 and p(z) = (z 3 ) 2 + (z 2 ) 2 + (z 3 ) 2 - The controls (Equation 4.39) asymp- 
totically stabilize the origin of Equation 4.38. For global stabilization the saturation 
functions are introduced in the control law. These functions eliminate the destabiliz- 
ing effects away from the origin. The control is thus given as 



i<i = —y x + o(p(z))(cosf- sinr) 
n 2 = — y 2 + CjC^ ) cos t 
n 3 =— y 3 + c 2 a(z 2 ) cost 
m 4 = -y 4 



(4.40) 



where c ; > 0 and a : R ^ R is a nondecreasing C 3 saturation function with a mag- 
nitude less than some 8 > 0 and is linear between (-8, 8). For global stabilization 8 
should be small enough. The saturation function then satisfies the following: 

1. g(z) = z when Izl < e 

2. lo(z)l < 8 for all z such that 0 < e < 8 

The closed-loop dynamics are given as 



Vi = — yi + o(p(z))(cos t - sin t) 
yi = -y 2 + CiO(zi) c osr 
v 3 =-y 3 +c 2 a(z 2 ) cost 

j 4 = 

= -yiyi + yicMzOcost 



(4.41) 



4 = - JiTs + yiC 2 o(z 2 ) cos t 



For some 0 < 8 < 8, 38 n such that if 8 < 8 0 , then the closed-loop dynamics are glob- 
ally asymptotically stabilized to zero. 



4.5.4 Simulation of the Controller 

For simulation the value of 8 is chosen to be 0.5 and the saturation functions 0(p(z)), alz,), 
and g(z 2 ) are 0.26, 0.5, and 0.1. The constants c, and c 2 are both chosen as 2. The initial 
values for y h y 2 , y 3 , and y 4 are chosen as -5, -2, -7, and -5, respectively. The simulation 
results are presented in Figures 4.25 through 4.34. 
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FIGURE 4.25 Point stabilization using time-varying feedback: x (m) vs. time (s). 
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FIGURE 4.26 Point stabilization using time-varying feedback: y (rad) vs. time (s). 
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FIGURE 4.27 Point stabilization using time-varying feedback: z (m) vs. time (s). 
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FIGURE 4.28 Point stabilization using time-varying feedback: \|/ (rad) vs. time (s). 
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FIGURE 4.30 Point stabilization using time-varying feedback: (f> (rad) vs. time (s). 
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FIGURE 4.33 Point stabilization using time-varying feedback: v 3 (rad/s) vs. time (s). 




FIGURE 4.34 Point stabilization using time-varying feedback: v 4 (rad/s) vs. time (s). 
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4.5.5 MATLAB Program Code for Point Stabilization 



% Point Stabilization feedback controller % 



t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
x=t ; 

xddot=l ; 
yd=a*sin (w*t) ; 
zd=l*ones (1,1001) ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t , y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t ,y3] =ode4 5 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode4 5 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3=yl . *y2 -0.5; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

2 ' 9 ' 5 ' 2 ' 2 ' 9 ' 9 ' 5 ' 5 ' 9 ' 5 ' 2 ' 2 ' 9 ' 5 ' 5 ' 5 ' 2 ' 2 ' 2 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 2 - 

ooooooooooooooooooooooooo'oo'o 



% Mat lab Functions 'ypl', 'yp2', 'yp3' and 'yp4'% 

2 ' 9 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 9 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 5 - 

oooooooooooooooooooooooooooo 

function yl = ypl(t,y) %%% function 'ypl' 
a= 1 ; w=pi; 
yl = zeros (3,1) ; 
y (1) =y(2) ; 
y (2) =y (3) ; 

y (3)=-15*y(3)-75*y(2)-125*y(l)+75*a*w*cos(w*t)- 

15*a*w*w*sin (w*t ) -a*w*w*w*cos (w*t) +125*a*sin (w*t) ; 

function y2 = yp2(t,y) %%% function 'yp2' 

a= 1 ; w=pi; 

y2 = zeros (3,1) ; 

y (1) =y(2) ; 

y ( 2 ) =y ( 3 ) ; 

y (3) =-15*y (3) - 4*y (2 ) -4*y ( 1) +a*w*sin (w*t ) +4*a*w*cos (w*t ) 
+4*a*sin (w*t) ; 

function y3 = yp2(t,y) %%% function 'yp3' 

a= 1 ; w=pi ; 

y3 = zeros (3,1) ; 
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y (l) =y (2) ; 
y (2) =y(3) ; 

y (3) =-4*y (3) -12*y (2) -4*y (1) + 4*t+15*a*w*w*sin (w*t) - 

a*w*w*w*cos (w*t) +125*a*sin (w*t) ; 

function y4 = yp4(t,y) %%% function 'yp4' 

a=l ; w=pi ; 

y4 = zeros (3,1) ; 

y (l) =y(2) ; 

y (2) =y (3) ; 

y (3 ) =-10*y (3 ) -12*y (2) -130*y (1) + a*w*sin (w*t ) + 
a*w*w*sin (w*t ) -a*w*w*w*cos (w*t) +75*a*sin (w*t) ; 



% Stabilization of 'x' position variable % 



t= [0:0. 01:120] ; a=l; w=pi ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3' , [t] , -7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 

% %% response for the 'x' variable %%% 
x=yl ; 



y=yl . *y2-0 . 5 ; 



z=yl . *y3-0 . 1; 

Figure (1) 

plot(t, x) , grid, title ( ' Stabilization of 



'x' variable') 



Stabilization of 'y' position variable 



t= [0:0. 01:120] ; a=l; w=pi ; 
x=t ; 

xddot =1 ; 
yd=a*sin (w*t) ; 
zd=l*ones (1,1001) ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
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% %% response for the 'y' variable %%% 
x=yl ; 

y=yl . *y2-0 . 5 ; 
z=yl . *y3-0 . 1; 

Figure (2) 

plot(t, y) , grid, title ( ' Stabilization of 'y' variable') 



5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 2 ' 2 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 9 ' 5 ' 2 ' 5 - 

ooooooooooooooooooooooooo 

% Stabilization of 'z' position variable % 

2 ' 5 ' 5 ' 9 ' 2 ' 5 ' 9 ' 5 ' 9 ' 5 ' 5 ' 2 ' 2 ' 5 ' 9 ' 5 ' 2 ' 5 ' 5 ' 9 ' 2 ' 5 ' 5 ' 5 ' 2 - 

ooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
x=t ; 

xddot=l ; 
yd=a*sin (w*t) ; 
zd=l*ones (1,1001) ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode4 5 ( 'yp4' , [t] , -5) ; 

% %% response for the 'z' variable %%% 
x=yl ; 

y=yl . *y2-0 . 5 ; 
z=yl . *y3-0 . 1; 

Figure (3) 

plot(t, z) , grid, title ( ' Stabilization of 'z' 
variable' ) 

5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 9 ' 5 ' 9 ' 5 ' 9 ' 9 ' 2 ' 5 ' 9 ' 2 ' 9 ' 5 ' 9 ' 9 ' 5 ' 5 ' 9 ' 2 ' 2 - 

ooooooooooooooooooooooooo 

% Stabilization of '\j/' position variable % 

5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 2 ' 9 ' 5 ' 9 ' 5 ' 2 ' 5 ' 2 ' 2 ' 2 ' 5 ' 2 ' 5 ' 2 ' 5 ' 5 ' 2 ' 2 - 

ooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 

xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot =1 ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 
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den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t , y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t ,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3=yl . *y2 -0.5; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

% %% response for the 'PSI' variable %%% 

PSI=atan (x2 ) ; 

Figure (4) 

plot(t, PSI), grid, title (' Stabilization of 'psi' 
variable' ) 

5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 - 

ooooooooooooooooooooooooo 

% Stabilization of '0' position variable % 

2 - 2 - 2 - 2 - 2 ' 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 ' 9 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 

xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot =1 ; 

yddot=a*w*cos (w*t) ; 

y2ddot= -a*w*w*sin (w*t ) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3 ' , [t] , - 7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3=yl . *y2 -0.5; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

% %% response for the 'Theta' variable %%% 

PSI=atan (x2 ) ; 
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theta=-atan (x4 . /sec (PS I ) ) ; 

Figure (5) 

plot(t, theta), grid, title (' Stabilization of 'theta' 
variable' ) 

5 ' 5 ' 9 ' 9 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 ' 5 ' 9 ' 5 ' 5 ' 5 ' 5 ' 9 ' 5 - 

ooooooooooooooooooooooooo 

% Stabilization of '())' position variable % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 9 - 2 - 9 - 2 - 2 - 2 - 

ooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
zd=l ; 

xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 

PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl; 
x2=y2 ; 

x3 =y 1 . *y2 - 0 . 5 ; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

% %% response for the 'phi' variable %%% 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PS I ) ) ; 

phi=acot (cot (theta) . /sin (PS I ) +tan (PS I ) . /sin (theta) ) ; 
Figure (6) 

plot(t, phi), grid, title (' Stabilization of 'phi' 
variable' ) 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooo 

% Stabilization of actual input v-^ 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooo 

t= [0:0. 01:120] ; a=l; w=pi ; 
xd=t ; 
zd=l ; 

xd2=a*w*cos (w*t) ; 
xd4 = 0*ones (1, 10000) ; 
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PSId=atan (xd2 ) ; 

thetad=-atan (xd4 ' . /sec (PSId) ) ; 

phid=acot (cot (thetad) . /sin (PSId) +tan (PSId) . /sin (thetad) ) ; 

xddot=l ; 
yd=a*sin (w*t) ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t ,y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3 =y 1 . *y2 - 0 . 5 ; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . / sec (PSI ) ) ; 

%%% Actual control variable v 2 %%% 
ul=-yl+0.26* (cos (t) -sin (t) ) ; 
u2=-y2+2*0 . 5* (cos (t) ) ; 
u3 = -y3 + 2*0.1* (cos (t) ) ; 
u4=-y4 ; 

vl=ul . / cos (PSI ) . *cos (theta) ; 

Figure (7) ; 

plot(t,vl), grid, title ( 'original system inputl (vl) ) ; 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Stabilization of input v 2 % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

ooooooooooooooooooooooooooo 

t= [0:0. 01:120] ; a=l; w=pi; 

xd=t ; 

xddot =1 ; 

yd=a*sin (w*t) ; 

zd=l*ones (1,1001) ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t,y3] =ode45 ( 'yp3 ' , [t] , - 7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl; 
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x2=y2 ; 

x3 =y 1 . *y2 - 0 . 5 ; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PS I ) ) ; 
ul=-yl+0.26* (cos (t) -sin (t) ) ; 
u2 = -y2 + 2*0 . 5* (cos (t) ) ; 
u3 = -y3 + 2*0.1* (cos (t) ) ; 
u4=-y4 ; 

%%% Actual control variable v 2 %%% 
rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 
v3=rll . * ( -r23 . *u2-r33 . *u3) ; 
v4=rll . * (r22 . *u2+r32 . *u3) ; 
v2=rlln . * (R1 . *u4-rl2 . *v3-rl3 . *v4) ; 

Figure (8) ; 

plot (t,v2),grid, title ( 'original system input2 (v2'); 

5 ' 5 ' 5 ' 2 ' 9 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 - 

oooooooooooooooooooooooooo 

% Stabilization of input v 3 % 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi ; 
xd=t ; 

yd=a*sin (w*t) ; 
xddot=l ; 

zd=l*ones (1,1001) ; 
yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t ) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t,y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t ,y3] =ode45 ( 'yp3 ' , [t] , -7) ; 

[t,y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3 =y 1 . *y2 - 0 . 5 ; 



© 2011 by Taylor and Francis Group, LLC 




114 



Autonomous Underwater Vehicles 



x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . / sec (PSI ) ) ; 
ul=-yl+0.26* (cos (t) -sin (t) ) ; 
u2 = -y2 + 2*0 . 5* (cos (t) ) ; 
u3 = -y3 + 2*0.1* (cos (t) ) ; 
u4=-y4 ; 

%%% Actual control variable v 3 %%% 
rll=cos (PSI ) . *cos (theta) ; 

r23=sin (PSI) . *sin (theta) . *cos (phi) -cos (PSI) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PSI) . *sin (theta) . *sin (phi) +cos (PSI) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PSI) . *sin (theta) . *sin (phi) -sin (PSI) . *cos (phi) ; 
rl3=sin (PSI) . *sin (theta) . *cos (phi) + sin (PSI) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 

v3=rll . * ( -r23 . *u2-r33 . *u3) ; 

Figure (9) ; 

plot (t,v3),grid, title ( 'original system input2 (v3)'); 

2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 ' 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 

oooooooooooooooooooooooooo 

% Stabilization of input v 4 % 

2 ' 5 ' 5 ' 2 ' 9 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 2 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 5 ' 2 - 

ooooooooooooooooooooooooooo 

t= [0 : 0 . 01 : 120] ; a=l; w=pi; 
xddot=l ; 

yddot=a*w*cos (w*t) ; 

y2ddot=-a*w*w*sin (w*t) ; yd=a*sin (w*t) ; 

den= (sqrt (xddot . *xddot+yddot . *yddot) ) . * (sqrt (xddot . *xddo 
t+yddot . *yddot ) ) . * ( sqrt (xddot . *xddot+yddot . *yddot ) ) ; 

[t , yl] =ode45 ( 'ypl' , [t] , -5) ; 

[t , y2] =ode45 ( 'yp2' , [t] , -2) ; 

[t , y3] =ode45 ( 'yp3 ' , [t] , - 7) ; 

[t , y4] =ode45 ( 'yp4' , [t] , -5) ; 
xl=yl ; 
x2=y2 ; 

x3=yl . *y2 -0.5; 
x4=y3 ; 

x5=yl . *y3-0 . 1; 
x6=y4 ; 

PSI=atan (x2 ) ; 

theta=-atan (x4 . /sec (PSI ) ) ; 
ul=-yl+0.26* (cos (t) -sin (t) ) ; 
u2 = -y2 + 2 * 0.5* (cos (t) ) ; 
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u3 = -y3 + 2*0.1* (cos (t) ) ; 
u4=-y4 ; 

%%% Actual control variable v 4 %%% 
rll=cos (PS I ) . *cos (theta) ; 

r23=sin (PS I) . *sin (theta) . *cos (phi) -cos (PS I) . *sin (phi) ; 
r33=cos (theta) . *cos (phi) ; 

r22=sin (PS I) . *sin (theta) . *sin (phi) +cos (PS I) . *cos (phi) ; 
r32=sin (phi) . *cos (theta) ; 

rl2=cos (PS I) . *sin (theta) . *sin (phi) -sin (PS I) . *cos (phi) ; 
rl3=sin (PS I) . *sin (theta) . *cos (phi) +sin (PS I) . *sin (phi) ; 
R=rll+r22+r33 ; 

Rl=l+R; 

rlln=l . / ( 1+rll ) ; 

v4=rll . * (r22 . *u2+r32 . *u3) ; 

Figure (10) ; 

plot (t,v4),grid, title ( 'original system input2 (v4)'); 
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In this chapter an overview of the dynamic mathematical model of an underwater 
vehicle is presented. The dynamic model is presented for the motion planning tasks. 
For the purpose of control design, the power form of the system is utilized. 



5.1 DYNAMIC MODELING 

In this section the dynamic model of the underwater vehicle is briefly discussed. 
The three-dimensional equations of motion for hydrodynamically shaped underwater 
vehicles can be developed using a body fixed coordinate frame and a global reference 
frame. The body fixed frame has components of motion given by the six velocity 
components v = [ v x , v y , v. , CG X , CO , CO. ], relative to a constant velocity coordinate frame 
moving with the ocean current, v c . The relative velocity is given as v r = v- v c (q), and 
for a constant ocean current v r =v and the velocity vector is represented as 

v = [v*,v r v,,®_ c ,co r coJ r (5.1) 

while the six components of position in the global reference frame are 

q = [p T|] r = [x,y,z,(|),0,\|/] r (5.2) 



The vehicle motion may be described in terms of the twelve nonlinear system 
equations [27] as 



M(t)v = f(v(t),q(t)Mt)) + g(v(t),q(t)u c (t) 
q = h(v(t),q(t),v c ) 



(5.3) 



The matrix M(t ) is a coupled mass matrix that includes both mechanical and hydro- 
dynamic added mass; the functions / and g, which are mappings of the vehicle 
motions into forces, including Coriolis, gravitational, and centrifugal forces; the 
hydrostatic and hydrodynamic forces and moments acting on the vehicle in the body 
fixed coordinate frame, with coefficient c; the motion-dependent influence of control 
surfaces, thrusters, and any ballasting; and the function h , which includes the kine- 
matical relationships found in performing the coordinate transformations between 
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body fixed and global reference frames and the constant ocean current, v c . The vec- 
tor u c (t) is the control input vector from control surfaces, propeller speeds, thruster 
forces, and buoyancy adjustment in general. The equations of motion can be written 
in the simplified form as 



Mv + C(v)v + D{v)v + g(q ) = C0((|)) + b(v,u) 
q=G{q)v 



(5.4) 



Here M is the inertial mass, including hydrodynamic virtual inertia or added mass; 
C(v) contains the nonlinear forces and moments due to centrifugal and Coriolis 
forces; D(v ) is the vehicle’s damping matrix, where the potential damping and viscous 
effect are lumped together; g(q) is the vector containing the restoring terms formed 
by the vehicle’s buoyancy and the gravitational terms; co(4>) is the wave and current 
disturbance vector; and b(y,u) is a vector containing the vehicle’s propulsion and con- 
trol forces and moments. For this chapter the disturbance in the system is ignored. 
In Chapter 6 the effect of disturbance is incorporated back while studying the robust 
control. Ignoring the effect of disturbance, the dynamic model is given as 



Mv + C(v)v + C A (v)v + D(v)v + g(q) = u c 

q=G(q)v 



(5.5) 



where the control vector is defined as u c = b(v,u) and q = G(q)v is the kinematic 
model derived in Chapter 3, given by Equation 3.13. The matrix G(q) is derived in 
Equation 3.12. Without any loss of generality, Equation 5.4 can be simplified as 



Mv = f(v,q) + u c 
q = G(q)v 



(5.6) 



5.2 POINT-TO-POINT STABILIZATION CONTROL DESIGN 

In the following section the control design for the problem of motion (point-to-point) 
stabilization is addressed. The system is supposed to reach a final desired configura- 
tion starting from an initial point, without the need to plan a trajectory. The control 
design task is to ensure the global asymptotic stability of the vectors q(t) and if/). 
In this section we design a feedback control for the dynamic model of the underwa- 
ter vehicles given by Equation 5.6 using the backstepping approach. The Lyapunov 
function will be used as a stability analysis tool as well as a feedback control design 
tool for this system. The design of feedback control is done in such a way that the 
Lyapunov function or its derivative has certain properties that guarantee bounded- 
ness or convergence to an equilibrium point. 
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5.2.1 State Feedback Control Using Backstepping 

Here we address the problem of designing a state feedback controller u c (t) that sta- 
bilizes the origin (q(t) = 0, v(t) = 0) of the control system (Equation 5.6). As stated 
in Chapter 4, the point stabilization for the kinematic model q — G(q)v cannot be 
achieved by a smooth time-invariant feedback. Only nonsmooth or time-varying 
feedback laws are of interest for the task. For our system we adopted the latter 
approach, where the time -varying smooth controller (Equation 4.39) was designed. 
The control law was designed utilizing the power form of the kinematic model. The 
power form as derived in Equation 4.38 is given as 

jl=Ki 

y 2 = u 2 

>3 = «3 

■0 (5-7) 

y 4 = u 4 ; z 4 o = u 4 

Z20 = yi «2 
^20 = yi“2 
230 = yi«3 



with the power form variables given as 



yi = *i 
y 3 = x 4 

y 4 = x 6 ; z 3 = z ° 4 0 = x 6 

Zl=Z20=-*3+*l*2 

z 2 =z\o=-x 5 +x l x 4 



(5.8) 



Here vector x = [x l ,x 2 ,x 3 ,x 4 ,x 5 ,x 6 \ T is the chained-form variables given by the 
coordinate transformation in Equation 3.23 as 



x, =^o = x 
x 2 = a 0 = tan\|/ 
x 3 = a, = y 
x 4 — T(q = — tan 0 sec \|/ 
x 5 = z 

1 



(5.9) 



Xf, = (r 3 2 - r 23 ) 

6 1 + tr(R) K 32 23 
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The vector u= [u l ,u 2 ,u 3 ,u 4 \ is the transformed input vector given in terms of the 
original velocity components v = [v x ,v y ,v z ,(S> x ,(S) ,coJ by the input transformation 
(Equation 3.25) as 



m, = costi/cosSv! 

u 2 = sec 2 \|/sin(|)secc0V3 + sec 2 \|/cos(|)sec0V4 

(- sin \|/ sin (|) sin 0 - cos \|/ cos (f») (sin \|/ cos § sin 0 + cos \|/ sin (f)) 

U 3 — v 3 v 4 

COS 2 \|/COS 2 0 COS 2 \|/COS 2 0 

(1+ cos \|/ cos 0) v 2 + (cos \|/ sin 0 sin <(> — sin \|/ cos(|)) v 3 + (sin V|/ sin 0 cos (|)+ sin \|/ sin (f>) v 4 



l + cos\|/cos0 + sin\|/sin0sin(|) + cos\|/cos(|) + cos0cos(|) 



(5.10) 



The power form (Equation 5.7) of the kinematic model q = G(q)v can be written 
in the simplified form as 



y = g(y) u 



(5.11) 



with vector >’= [>’]•■ V2 » V3 » V4 ^ ^1 ^ ^2 1 7 denoting the power form variables and vector 
u = [u l ,u 2 ,u 3 ,u 4 ] r denoting the transformed velocity input vector. Using the power 
form of the kinematic model and the dynamic model. Equation 5.6 can be written as 



Mil = f(u,q) + u c = u 

}’ = g(y)u 



(5.12) 



where m = f(v,q) + u c is the control variable. As stated earlier, the problem of motion 
stabilization for Equation 5.12 is to design a state feedback controller u{t ) that sta- 
bilizes the origin (y(t) = 0 ,u(t) = 0) of the control system (Equation 5.12). More spe- 
cifically we consider the control law 

u(t) = F(y,u ) (5.13) 



such that the origin of closed-loop dynamics is exponentially stable, with F being a 
nonlinear operator. The control strategy adopted here is similar in principle to feed- 
back control by backstepping for ordinary differential equations [26]. Backstepping 
is a Lyapunov-based control method of feedback linearization. It is a recursive 
method that designs the feedback control law based on the choice of the Lyapunov 
function. It breaks the design problem for a system of equations into a sequence of 
design problems for scalar systems. We proceed with the control design as follows: 

5.2.2 Control with Smooth Time-Varying Feedback 

First we proceed to design a conceptual control law u = H(y,t) for Equation 5.11 to 
stabilize the origin y(t) = 0, with H(y,t) being a nonlinear map. Here u(t) is an input 
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to the power form of the kinematic model. This smooth time-varying control law was 
designed in Chapter 4 and is given by Equation 4.40 as 

M i = - >’i + ct(p(z))(cosr- sinf) 

u 2 = -y 2 + c 1 a(z l )cost (5 U) 

u 3 = —y 3 + c 2 o(z 2 ) cos f 
«4 = -.V 4 

where c,- > 0 and a :/?—»/? is a nondecreasing C 3 saturation function with a mag- 
nitude less than some 8 > 0 and is linear between (- 8, 8). For global stabilization 8 
should be small enough. The saturation function then satisfies the following: 

1. a(z) = z when Izl < 8 

2. Ig(-)I < 8 for all z such that 0 < e < 8 



With the controller (Equation 5.14) we get the conceptual closed-loop dynamics 
for Equation 5.11 as 

>’i = -}’| + ct(p(z))(cost- sinf) 
ih = -y2 + CiO(z 1 )cosf 

y 3 =-3; 3 + C2 0(z 2 )COSf 5 

,V4 = -y* 

ii = -yi.y 2 + yiqaCz^cosf 
z 2 = -yi.y 3 + ,yic 2 o(z 2 )cosr 



5.2.3 Lyapunov Stability Analysis 

In order to check the stability of the closed-loop system (Equation 5.15), we use the 
Lyapunov function analysis method. The stability problem is to establish sufficient 
conditions for which the origin y(t) = 0 of the closed-loop dynamics (Equation 5.15) 
is globally asymptotic stable. Within the framework of our system the definition of 
Lyapunov stability can be given as follows: 

Definition: An equilibrium state y = 0 of a dynamical system 
stable if for each real number e > 0, there exists a real number 8 = 

||y(f 0 )||<S=>ll;y(f)ll<e Vr>f 0 

If in addition 8 can be chosen such that 

llv(f 0 )ll<8=>limy(f) = 0 (5-17) 



(Equation 5.15) is 
= 8(8) > 0 such that 

(5.16) 



then the equilibrium is said to be asymptotically stable. 
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The stability of an equilibrium point y = 0 of a dynamical system (Equation 5.15) can 
also be evaluated by defining a Lyapunov function V(t) for this system. Here V : D — > 91 
is a continuously differentiable function defined in a domain D a 9i 6 that contains the 
origin. In terms of the Lyapunov function the stability can be defined as follows: 

Definition: An equilibrium state y = 0 of a dynamical system (Equation 5.15) is 
stable if there exists a continuously differentiable function V : D — » 91 such that 

V(0) = 0 and V(y) > 0 VyAO (5.18) 

V(y) < 0 \/yeD (5.19) 



If in addition 

V(y) < 0 Vy^O (5.20) 



then the equilibrium is said to be asymptotically stable. 

Let us consider a Lyapunov function V : D — > 91 for the system in Equation 5.15 as 

V(y,t) = ^y T (t)y(t) (5.21) 



Since V(y) is a quadratic function of V(t), we can easily conclude that V(y) is a posi- 
tive definite function. The time rate of change of V(y) is given as 



V(y,t) 



dV(y) 

dy 



y(o=Z 



dV(v) . 

dy, ■ Vi 



(5.22) 



Using the center manifold theorem, it can be proven that the derivative of the 
Lyapunov function V(y) given by Equation 5.22 for the closed-loop dynamics 
(Equation 5.15) is a negative definite function. Hence, it can be concluded that the 
equilibrium point of dynamics given by Equation 5.15, using the control law in 
Equation 5.14, is asymptotically stable. Also, for some 0 < e < 8, 38 0 such that if e < £ 0 , 
the closed-loop dynamics are globally asymptotically stabilized to zero and V(y) is 
bounded by a negative definite function —W(y). The detailed proof is given in [28]. 



5.2.4 Control of the Dynamic Model 

In the previous section we established the asymptotic stability of the origin y(f) = 0 
for the kinematic model 



y = g(y)u (5.23) 

using the conceptual control law 

u(t) = H(y,t ) (5.24) 
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so that the closed-loop dynamics are given by Equation 5.15 as 

y=g(y)H(y,t) (5.25) 

As already shown, this closed-loop system is asymptotically stable with a conceptual 
Lyapunov function (Equation 5.21) 

V(y,t) = ^y T (t)y(t) 

We have used the term conceptual with the control law, closed-loop system, 
and Lyapunov function. This is done in order to stress the fact that the control 
law in Equation 5.24 cannot be implemented in practice, as w(f) is not a control 
variable. However, this conceptual design helps us to recognize the benefit of the 
input u(t) being close to H(y,t). From the knowledge of the conceptual Lyapunov 
function V(t ) we want to design a smooth feedback control for stabilizing the 
origin of the overall system (Equation 5.12). We therefore add to the conceptual 
Lyapunov function (Equation 5.21) a term penalizing the difference between u(t) 
and H(y,t). For this purpose, we rewrite the dynamics in Equation 5.24 as 

y = g(y)H(y,t ) + g(y){u(t) - H (y,t ) ) (5.26) 

Defining the difference between u(t) and H(y,t) by an error variable /(f) u{t) - H(y,t), 
we get the following modified dynamics: 

m = 8(y)H(y,t) + g(ym (5-27) 

m = (5.28) 

Substituting the dynamic model in Equation 5.12 in Equation 5.28, we get the fol- 
lowing dynamic model: 



y(f) = g{y)H{y,t) + g{y)l(t) (5.29) 

kt)= U ^-H{y,t) = u n {t) (5.30) 



where u n (r) = - H(y,t) is the new control variable. The above system is similar 

to the system in Equation 5.12, except that the first component (Equation 5.29) has an 
asymptotically stable origin when its input /(f) is zero. The time derivative of func- 
tion H(y,t) can be computed using the following expression: 



H(y,t) = ——y(t) 
ay 



-rg{y)u 

dy 



(5.31) 



© 2011 by Taylor and Francis Group, LLC 




124 



Autonomous Underwater Vehicles 



Now let us modify the Lyapunov function (Equation 5.21) by adding an error term 
to it, thus resulting in the Lyapunov function for the overall system as 



V a (y,u) = V(y) + ^Fmt) 

= \y T (t)y(t) + ^l T (t)l(t) 



(5.32) 



The time rate of change of this functional using Equations 5.29 and 5.30 is given as 



V a (y,u) 



— y(t) + l T mt ) 
dy 



dy 



[g(y)H(y,0] + 



dv 

dy 



[giy)l(t)] + F(t)u n (t) 



(5.33) 



The first term in the above equation is bounded by a negative definite function —W{y)\ 
therefore, we have 



V a (y,u)<-W(y) + 



dv_ 

dy 



[g(y)l(t)] + F(t)u n (t) 



(5.34) 



We have to choose a new control law u„(t) in such a manner that the time deriva- 
tive of the new function or the sum of the second and third terms is also bounded by 
a negative definite function. By choosing the control law 



u n (t) 



, g(y)-klit), k> 0 

dy 



(5.35) 



we get the following result: 



V a (y,u)<-W(y)-kF(tm (5.36) 



Since the product F(t)l(t ) is positive definite, with k > 0, the origin (y(t) = 0, lit) = 0) 
of the system in Equations 5.29 and 5.30 is asymptotically stable. The feedback control 
law is given by 



= - g(y)-klit) 
dy 



dV(y) 



g(y)-ki(t ) 



^ dy,- 

i=l Jl 
6 

■^yig(y)- kl(t) 



(5.37) 
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where g(y) is the nonlinear system function for Equation 5.7 and lit) = u(t) - H(y,t), 
with w(f)being the control law given by Equation 5.14. The feedback controller for 
the original system (Equation 5.12) is given by 



~~ = u n (t) + H(y,t) 
u(t)= M[u„(t) + H{y,t)] 



(5.38) 



Using Equation 5.31, 



H(y,t) = —m 

ay 



-y-g(y)u 

dy 



the control law u(t) can be computed as 



m = M[u n (t ) + — g(y)u(t)] (5.39) 

dy 

with the control u n (t) given by Equation 5.37 and the control law u(t) given by 
Equation 5.14. With the feedback controller (Equation 5.37) we have proved that the 
origin (y(t) = 0 ,/(r) = 0) of Equations 5.29 and 5.30 is asymptotically stable. But we 
need to prove the asymptotic stability of the origin (y(?) = 0 ,u(t) = 0) of the system 
(Equation 5.12), or we need to prove the asymptotic stability of u(t) = 0 using the 
control law in Equation 5.39. The proposition below establishes the asymptotic sta- 
bility of the original closed-loop system (Equation 5.12). 

Proposition: The origin (y(t) = 0 ,u(t) = 0) of the system in Equation 5.12 is asymp- 
totically stable under the control law u(t) = M[u n (t) + with u„(t) given 

by Equation 5.37 and control law u(t) given by Equation 5.14. 

Proof: The control laws in Equation 5.37 make the origin (y(t) = 0 ,l(t) = 0 for 
Equations 5.29 and 5.30 asymptotically go to zero. From the equation l(t) = u(t ) - 
H(y,t), we want to prove the asymptotic stability of u(t) = 0. We know that the control 
H(y,t ) is zero for y(t) = 0, that is, H( 0) = 0. We can conclude that the origin (y(t) = 0 
u(t) = 0) is asymptotically stable using the control law in Equation 5.39. 
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In previous chapters we addressed the control of underwater vehicles without 
accounting for the presence of uncertainty in the design of the controller. The uncer- 
tainty is a mismatch between the model used for controller design and the actual 
process model. The uncertain function may include uncertain model parameters or 
external disturbances. Here we consider the case where we have an uncertainty in 
the input to the system. The objective is to develop a framework for the synthe- 
sis of robust controllers that handle the effect of this uncertain variable for both 
kinematic and dynamic models of an underwater vehicle. The robust controllers 
designed guarantee boundedness of state and achieve asymptotic stabilization with 
an arbitrary degree of asymptotic attenuation to the effect of uncertain variables on 
the output of the closed-loop system. The controllers are designed constructively 
using Lyapunov’s direct method [31] and require the existence of known bounding 
functions that capture the magnitude of the uncertain term. This chapter presents 
the design of robust nonlinear feedback controllers for models given in Chapters 
3 and 5 representing kinematic and dynamic models for underwater vehicles, with 
uncertainty in the control input. In both cases the objective of control design is to 
synthesize nonlinear feedback controllers that stabilize the system and guarantee the 
stability of the closed-loop system in the presence of uncertainty. 

6.1 ROBUST CONTROL USING THE KINEMATIC MODEL 

In this section we formulate the uncertain control model and present robust control 
design for the kinematic model derived in Chapter 3. The problem is to design a 
state feedback control for the problem of motion (point-to-point) stabilization. The 
control design task is to ensure the global asymptotic stability of the vector q(t) of 
the closed-loop system irrespective of uncertain elements. The kinematic model is 
given by Equation 3.13 as 



q = G(q)v 



( 6 . 1 ) 



Here the vector q = [p r|| 7 = [v, _v,z,(|).0.'|/] 7 is the six components of position in the 
global reference frame variable we want to control. The body fixed frame has compo- 
nents of motion given by the six-velocity-component vector v = [ v v , v y , v. , CG X , co v , CO.], 
relative to a constant velocity coordinate frame moving with the ocean current, v c . As 
stated in Chapter 4, the point stabilization for the kinematic model q = G(q)v cannot 
be achieved by a smooth time-invariant feedback. Only nonsmooth or time-varying 
feedback laws are of interest for the task. For our system we adopted the latter 
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approach, where the time-varying smooth controller (Equation 4.39) was designed. 
The control law was designed utilizing the power form of the kinematic model. The 
power form as derived in Equation 4.38 is given as 

jl = Mj 



y 2 = u 2 
y 3 = u 3 

(6.2) 

y A = u A \ = u A 
^20 = 

2 m = yvi 



with the power form variables given as 

y\ = *i 



y 2 = x 2 



,v 3 = x 4 

o (6 - 3) 

y 4 = x 6 , z 3 = z 40 = x 6 

21 = 220 = *3 + X X X 2 

22 = 230 = --*5 + * 1*4 

Here vector x = [x l ,x 2 ,x 3 ,x A ,x 5 ,x 6 ] T is the chained-form variables given by the coor- 
dinate transformation in Equation 3.23. The vector u = [m, , u 2 , u 3 , u A ] is the transformed 
input vector given in terms of the original velocity components v = [ v x , v , v z , CO* , C0 y , CO J 
by the input transformation (Equation 3.25). The power form (Equation 6.2) of the 
kinematic model q = G(q)v can be written in the simplified form as 

y = g(y)u ( 6 . 4 ) 



with vector y= [y 1 ,y 2 ,y 3 ,y 4 ,Zi,Z 2 Y denoting the power form variables and vector 
u = [u u u 2 ,u 3 ,u a ] t denoting the transformed velocity input vector. 

6.1.1 Input Uncertain Control Model 

Let us consider that the uncertain variable for this system is the control input or the free 
flow velocity. This means that there is an error between the control command and the 
actual input command to the system. The uncertain model is therefore given as 

y = sOO[«(O+0] (6.5) 
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In the above equation 0 = 9(t,u,y) denotes the unknown function, which takes care of 
uncertainty in the input u(t) to the system. The uncertain term here satisfies an impor- 
tant structural property; namely, it enters the state equation exactly at the point where 
the control variable enters. This property will be referred to as the matching condition. 
The nominal model (the system without uncertainty) for this system is described by 

y = §(y)u 



The first step is to design a stabilizing feedback controller for the nominal model. 
We proceed to design a conceptual control law u(t) = H(y,t) for Equation 6.4 to 
stabilize the origin y(f) = 0, with H(y,t) being a nonlinear map and u(t) an input to 
the power form of the kinematic model. This smooth time -varying control law was 
designed in Chapter 4 and is given by Equation 4.40 as 



m, = -y, + 0 (p(z))(cosr- sinf) 
u 2 = —y 2 + C| 0 (z, ) cos t 
n 3 = —y 3 + c 2 o{z 2 ) cos t 



(6.6) 



«4 = -y* 

where c, > 0 and a: R — > R is a nondecreasing C 3 saturation function with a mag- 
nitude less than some 8 > 0 and is linear between (-8,8). For global stabilization 8 
should be small enough. The saturation function then satisfies the following: 

1. a(z) = z when Id < e 

2. Ia(z)l < 8 for all z such that 0 < e < 8 

With the controller (Equation 6.6) we get the nominal closed-loop dynamics for 
Equation 6.2 as 

>’i =->’i +o(p(z)Xcos?-sinf) 
jh =-y 2 + CiCT(zi)cosf 
j 3 = -3; 3 + C2 o(z 2 )cosr 

(6.7) 

y 4 = -y 4 

Zi = -y&2 + ^qotz^cost 
ii = -T1J3 + ,yic 2 o(z 2 )cosr 



Thus, with \Q(t,u,y')\ = 0 and n(f) = H(y,t) the nominal closed-loop system y(t) = g(y) 
H(y,t) given by Equation 6.7 has a globally asymptotic stable origin, as proved in 
Section 5.2.3, and there exists a Lyapunov function given by Equation 5.21, 

V{y,t)= ] 2 y T {t)y(t ) 
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whose time derivative V(y) is bounded by a negative definite function - W (y) as 



V(y)<-W(y) 



( 6 . 8 ) 



with W(y) being a positive definite function. 



6.1.2 Robust Control by the Lyapunov Redesign Method 

In this section we consider the system of Equation 6.5 and address the problem 
of synthesizing a state feedback controller that stabilizes the closed-loop system 
irrespective of the uncertainty. The controller is designed constructively using 
Lyapunov’s direct method, where we use the Lyapunov function for the system to 
design feedback control. A standard method for finding a Lyapunov function for 
an uncertain system is developed in [29] and is known as Lyapunov redesign. This 
technique has been incorporated in various books like [30], [31], and [32]. The key 
idea of this method is to employ a Lyapunov function for the nominal system as a 
Lyapunov function for the uncertain system. This reuse of the same Lyapunov func- 
tion is referred to by the term redesign. The Lyapunov redesign technique uses a 
Lyapunov function of a nominal system to design an additional control component 
to robustify the design to a class of large uncertainties that satisfy the matching con- 
dition, that is, the uncertain terms enter the state equation at the same point as the 
input. Lyapunov redesign can be used to achieve robust stabilization. The goal is to 
design a feedback control law for Equation 6.5 as 

u(t) = H{y,t)+G(y,t) (6.9) 



such that we achieve closed-loop stability and asymptotic attenuation of Q(t,u,y), 
where G(y,t) is a nonlinear operator mapping. In Equation 6.9 H{y,t ) achieves 
closed-loop stability and G(y,t) asymptotically attenuates the effect of Q(t,u,y). 
The function H(y,t ) will be designed by the previous approach, and G(y,t ) will 
be designed using the Lyapunov redesign method. The design of function G{y,t) 
is known as Lyapunov redesign [26] and is done on the basis of the assumption 
that we have a bounding function that captures the size of the disturbance. Let 
us assume with the controller in Equation 6.9 that there exists a known smooth 
function that bounds the magnitude of uncertain variables as 



\\e(t,u,y)\\ = \m,y,(H(y,t)+G(y,t))\\ 

<y(t,y) + K\\G(y,t))W 



( 6 . 10 ) 



where y(t,y) is a nonnegative function and is a measure of size of uncertainty 9 (t,u,y). 
From Equation 6.10, the only requirement is the knowledge of y (t,y), which doesn’t 
necessarily have to be small. From the knowledge of the Lyapunov function V(1) and 
functions y(t,y) and K the goal is to design G(y,t) and apply u(t) = H(y,t) + G(y,t) to the 
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actual system (Equation 6.5) such that the overall closed-loop system is stabilized in 
the presence of uncertainty. By using the control law in Equation 6.6 the feedback 
control law (Equation 6.9) is given by 

u(t) = H(y,t) + G(y,t) 



Under the feedback control law the closed-loop dynamics for Equation 6.5 take 
the form 



y(t) = g(y)H(y,t)+ g(y)G(y,t) (6.11) 



Thus, the error in the nominal closed-loop dynamics y(t) = g(y)H(y,t) given by 
Equation 6.7 and the closed-loop dynamics (Equation 6.11) due to input uncertainty 
is given by 



Q = g(y)G(y,t) 



( 6 . 12 ) 



with g(y) given by Equation 6.2 as 



g(y) = 



1 0 

0 1 

0 0 

0 0 

0 )>, 

0 0 



0 0 

0 0 

1 0 

0 0 

0 0 

>i 0 



0 0 
0 0 
0 0 
1 0 
0 0 
0 0 



(6.13) 



Let us denote the component G(y,t) of control input in Equation 6.9; by v we can 
rewrite Equation 6.12 as 



0=g(y)v 



The magnitude of this uncertain term is given by 

11011 = IlgOOvll (6.14) 



The bound on the magnitude of uncertainty (Equation 6.14) can be found as 



Il9(f,y,w)ll < llg(y)ll llvll 
= y(y,t) + K(y,t)v 



(6.15) 
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where K (y,t) = y\, y(y,t) = llg(y)ll . This bound will be utilized to design the control 
law G(y,t). Let us now apply the control law in Equation 6.9 to the input uncertain 
system (Equation 6.5) so that the closed-loop dynamics take the form 

y(t) = g(y)H(y , t) + g(y)[G(y,t) + Q(t, y, u)\ (6.16) 



As can be seen, the system in Equation 6.16 is a perturbation of the nominal closed-loop 
system (Equation 6.7), the third term being the perturbation. Let us choose the Lyapunov 
function V(y,t)= \ y r (t)y(t) for Equation 6.16, same as before. To design G(y,t) we 
find the rate of change of the Lyapunov function as 



V(y,t) = 
V(y,t) = 



< 



dV(y) . 
dy 



av(y) . 



1=1 Jl 



{g(y)H(y,t) + g(y)[G(y,t) + e(t,y,u)]} 



dV(y) 

dy 

' my> [ g(y)H(: y, 0] + dV J y) {g(y)[G(y, t)+Q(t,y , «)] } 
dy dy 

-W(y)+ r)V ^ y) {g(y)[G(y, t)+Q(t,y , «)] } 
dy 



(6.17) 



The first inequality is because of the asymptotic stability of the nominal closed- 
loop system (Equation 6.7). We need to choose a control law G(y,t) so as to cancel 
the destabilizing effort of Q(t,u,y ) on V(y,t). The law G(y,t) should be such that the 
second term in Equation 6.17 is negative semidefinite in order to have asymptotic 
stability. By denoting Giy,t) by v we can rewrite Equation 6.17 as 

V(y,t) < -W(y)+ r) ^ y) [g(y)v(t)]+ r) ^ y) [g(y)Q(t)] (6.18) 

V(y, t) < -W(y) + Y 3 ^g(y) v(r) + Y (6.19) 

m dy ‘ m dy ‘ 



By using Equation 6.15 we have the rate of change of Lyapunov bounded as 

V(y, t ) < -W(y) + ^ ^pW)v(f) + £ 3 p%O0{Y()', 0 + K(y, r)v} ] 

(=i y ‘ i=i y ‘ 

< - W(y) + ^ 3 p^g(y)[l + K(y,/)]v(0 + ^ ^p^[g(y>y(y,f)] 

r=i i = i 



( 6 . 20 ) 
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Denoting 



w T iy) 



dv(y) 

dy 



o 

s(y)=X 



dV(y ) 

dy t 



g(y) 



( 6 . 21 ) 



we can rewrite Equation 6.20 as 

V(y,t)<-W(y)+w T (y)tt + K(y,t)]v(t)+w T (y)y(y,t) (6.22) 



Now let us choose the state feedback control as follows: 



v(0 



TjOvO 

1-k (y,t) 



w(y,t) 



(6.23) 



where r|(y,f) > y(y,t) for all t > 0 is a nonnegative function. By using this control 
law it is seen that the sum of the second and third terms in Equation 6.22 is zero, 
that is. 



w r (y)[l + K(y, f)]v(f) + w T {y)y{y,t) = 0 (6.24) 

By using the control law in Equation 6.23 and then applying Equation 6.24 we can 
show that 



V(y,t)<-W(y)) 



Hence, with the control law in Equation 6.23, the derivative of V(y,t) for the closed- 
loop system (Equation 6.16) is negative definite and we have asymptotic stability in 
the presence of the disturbance. The robust control law for Equation 6.5 is therefore 
given as 



w(f) = H(y,t) - w(y,t) 

1-k (y,t) 



(6.25) 



6.2 ROBUST CONTROL USING THE DYNAMIC MODEL 

In this section we formulate the control model and present a robust control design 
for the dynamic model of an underwater vehicle. This section also studies Lyapunov 
stability for this model with the uncertainty in the input to the system. The control 
design is done using a combination of both Lyapunov redesign and backstepping. 
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and the technique is referred to as robust backstepping. Let us consider the dynamic 
model for an underwater vehicle given in Section 5.21 by Equation 5.12 as 

Mii = f(u,q) + u c = ii (6.26) 

y = g(y)u (6.27) 

where u = f(v,q) + u c is the control variable. Here y(t) and u(t) are the variables we 
want to control. For point-to-point stabilization we require y(t) = 0 and u(t) = 0. 

Now let us consider the uncertainty in the control input for this system. We can 
have both matched and unmatched uncertainties, depending upon which equation 
contains the uncertainty. We can have uncertainty in Equation 6.27 where it enters the 
equation through the control variable n(f). For this case the uncertainty is unmatched 
since u(t) is the conceptual control input to this equation and is not the actual control 
input as in Equation 6.26. We can also have uncertainty in the control variable u(t), in 
which case the uncertainty is matched and we can also have both. The control design 
scheme in all cases is that of robust backstepping [32], where we do a combination of 
backstepping and Lyapunov redesign. Now let us discuss these cases one by one. 

6.2.1 Robust Backstepping: Unmatched Uncertainty 

Let us first consider the uncertainty in the input u(t) to the nominal system in Equation 
6.27. The control design strategy here is robust backstepping, which is the combina- 
tion of backstepping and Lyapunov redesign. The uncertain model is given as 



Mu = u (6.28) 

j=g(y)[«(r) + ei (6.29) 

where 0 = 0 (t,u,y) is the unknown function that takes care of the disturbance in the input 
u(t ) to Equation 6.27. The uncertain term for the dynamic system in Equation 6.28 is 
unmatched, as it does not enter the system at the same point as the input u, even though it 
enters equation Equation 6.26 through the conceptual input u(t). As a first step, we design 
a robust control for Equation 6.29 using the Lyapunov redesign technique, and in the next 
step find the control law for the overall system (Equation 6.28) using backstepping. 

1. Lyapunov redesign: The first step is to design a robust controller for 
Equation 6.29 using the Lyapunov redesign method. The goal is to design a 
robust control law for Equation 6.30 as 



u(t) = H(y,t)+ v(t)= H{y,t) (6.30) 

such that we achieve closed-loop stability and asymptotic attenuation of 
0(r,M,y). H(y,t ) in Equation 6.30 achieves closed-loop stability and v(r) asymp- 
totically attenuates the effect of Q(t,u,y). The function H(y,t ) is designed 
from the feedback linearization of the nominal model in Equation 6.27 for 
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Equation 6.29, and v(t) will be designed by using the Lyapunov redesign 
method. The nominal model (Equation 6.27) is given as 

y = g(y)u 



The stabilizing feedback controller u(t) = H(y,t ) for this nominal model 
from Equation 4.40 given in Chapter 4 is 



Mi = ~y 1 + o(p(z))(cosr- sinf) 
m 2 = —y 2 + c\o(zi ) cos t 
m 3 = —y 3 + c 2 o(z 2 ) cos t 



(6.31) 



n 4 = -y 4 

where c,- > 0 and o : R — > R is a nondecreasing C 3 saturation function with 
a magnitude less than some 8 > 0 and is linear between (-8,8). For global 
stabilization 8 should be small enough. The saturation function then satis- 
fies the following: 

1. a(z) = z when Id < e 

2. Ia(z)l < 8 for all z such that 0 < e < 8 

which gives the conceptual nominal closed-loop system as given by 
Equation 6.7: 



y(t) = g(y)H(y,t) (6.32) 

The nominal closed-loop model (Equation 6.32) has an asymptotically stable 
origin, and there exists a Lyapunov function V(t) given by Equation 5.21 

V(y,t) = ^y T (t)y(t) 



which satisfies 

V(y)<-W(y) (6.33) 

with Vk(v) being a positive definite function. Now by using Equation 6.31 
the control law (Equation 6.30) becomes 

u(t) = H(y,t)+v(t) (6.34) 

which gives the following closed-loop dynamics for Equation 6.27: 

y(t) = g(y)H(y,t) + g{y)v(t) (6.35) 
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Thus, the error in the closed-loop dynamics (Equations 6.32 and 6.35) due 
to input uncertainty is given by 

d(t,y,u) = g(y)v(t) (6.36) 

with g(y) given by Equation 6.13. The magnitude of this uncertain term due 
to Equation 6.15 is 



\\%(t,y,u)\\ < llg(y)ll llv(f)H 
= y (y,t)+K(y,t) iiv(OM 



(6.37) 



From the previous subsection we found the control law v(t) using Lyapunov 
redesign, given by Equation 6.23 as 

v(t) = - riy ' t) W (y,t) (6.38) 

1-K(y,f) 

where q(y,f) > J(y,t) for all t > 0 is a nonnegative function. By using this 
control law, it is seen that the closed-loop system in Equation 6.29 has 
asymptotic stability in the presence of the disturbance. The robust control 
law for Equation 6.29 is therefore given as 

u(t) = H(y,t)~ Tl(y ’° w(y,t) (6.39) 

1-K(y,r) 

2. Control by backstepping method: The next step is to design a feedback 
controller for the overall system (Equation 6.28) using the backstepping 
method described in Section 5.2.1. The goal is to a design feedback con- 
trol law u(t) to stabilize the overall system from the knowledge of the 
Lyapunov function V( 1 ) for Equation 6.29 and modify it. We proceed with 
the control design in a similar way as in Section 5.2.1, as follows: 

a. First design a conceptual control law u(t) — H(y,t ) for Equation 6.29 
from the previous step to stabilize origin y(t) = 0. With this control law 
the conceptual closed-loop dynamics for Equation 6.29 are asymptoti- 
cally stable and there exists a Lyapunov function (Equation 5.21) that 
satisfies V(y,t) < — W(y). 

b. Since u(t) is not the actual control variable, defining the difference 
between u(r) and H(y,t ) by an error variable l(t)= u(t) — H(y,t), we 
get the following modified dynamics: 

y(t) = g(y) [H(y,t) + l(t)] (6.40) 

i(t) = u n (t) (6.41) 

where u n (t) = — H(y,t) is the new control variable. 
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c. Now let us modify the Lyapunov function by adding the error term to it 
as in Equation 5.32, which is given by 

V a (y,u) = V(y)+' 2 r'(t)l(t) 



with /(f) = u(t) - H(y,t). The control law ujt) is given by Equation 5.37 as 

u„(t) = -^ g(y)-kKt) 

ay 



1=1 Jl 



(6.42) 



O 

-^yig(y)- kl(t) 



This control ensures that the origin of the system (Equations 6.40 and 6.41) 
is asymptotically stable. 

Thus, the final robust feedback control law for Equations 6.28 and 6.29 
is given by the following equation: 



u(t) = M[u n (t) + 



—g(yMt)] 

dy 



(6.43) 



with u n (t) given by Equation 6.42 and 



u(t) = H(y,t) = H(y,t)— w(y,t ) 

1 — K(y,f) 



(6.44) 



6.2.2 Robust Control: Matched Uncertainty 

Let us consider the uncertainty in the input to the overall system (Equation 6.26) 
(7(f). By assuming the uncertainty in input (7(f) in the second equation we have our 
input uncertain model as 



Mu = u(t) + Q(t) (6.45) 

y = g(y)u(t) ( 646 ) 

Here the uncertainty is matched as it enters the system at the same point as the 
actual control input u(t). With I9(f,w,y)l = 0 the nominal model for this system is 
given by Equations 6.26 and 6.28. As a first step toward the control design we design 
feedback control for the nominal model (the system without disturbance) by applying 
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the backstepping technique. After that the robust control for the uncertain system 
(Equation 6.45) is designed using the Lyapunov redesign technique. We proceed 
with the control design as follows: 



1. Control by backstepping method: The first step is to design a feedback 
controller u(t) = F(y,u) for the nominal dynamic system (Equations 6.26 
and 6.28) using the backstepping technique. The controller as designed in 
Section 5.2.4 is given by Equation 5.39 as 



u(t)= M[u n (t) + 



—g(y)u(t)\ 

dy 



(6.47) 



with the control u n {t) given by Equation 5.37 as 
u n (t) = -^g(y)-kl(t) 

ay 



dV(y) 



■g(y)-ki(t ) 



^ dy,- 

i=l Jl 
6 

^y.-sOO- ki(t) 



(6.48) 



where g(y) is the nonlinear system function given by Equation 6.13 and 
/(f) = u{t) — H(y,t), with u(t) being the control law given by Equation 6.6. 
The nominal system (Equations 6.26 and 6.28) is asymptotically stable 
with the control law in Equation 6.47, and there exists a Lyapunov function 
VJy,t ) given by Equation 5.32 as 



V a (y ,u) = V(y)+^l T mt) 

= ^y T (t)y(t)+^l T mt) 

whose time derivative satisfies the following inequality: 
V a (y,u)<-W{y)-kl T {tm,k> 0. 



(6.49) 



(6.50) 



2. Lyapunov redesign: The next step is to design a robust control for the 
uncertain model (Equations 6.45 and 6.46) using the Lyapunov redesign 
method. The goal is to design a feedback control law 



u(t') = u(t)+v(t) 



(6.51) 
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such that we achieve closed-loop stability and asymptotic attenuation of 
0(f,y,n). u(t ) is given by Equation 6.47, and v(f) will be designed by 
using the Lyapunov redesign method. Now with the feedback controller 
u(t) — u{t) + v(t) the error in the closed-loop dynamics for the nominal and 
actual models due to the input uncertain term is 

0(f,y,n) = v(f) 



The magnitude of this uncertain term is given by 

11011 = II v(f)ll (6.52) 

The bound on the magnitude of uncertainty (Equation 6.52) can be expressed 
as 

ll0(f,y,n)ll<K(y,f)llv(r)ll (6.53) 



where K(y,f) > 0. After applying the control law in Equation 6.51, the rate 
of change of the Lyapunov function VJt) is 



V„(y,u) 



— y{t) + l r mt) 

oy 



with 

/(f) = u(t)~ H(y,t) 



and 



i(t) 



u(t ) 

M 



H(y,t) + 



v« + A 

M M 



Thus, the time derivative of V a (t ) is given as 



v a (y,u) 



dv_ 

dy 



[g(y)H(y,t)\ + F(t) 



u(t) 

M 



■ v(t) 0 

H(y,t)+— + — 
M M 



Using the inequality in Equation 6.50, we have 



V a (y,u)<-W(y)-kl T mt)+l T (t) 



v(0 + A 

M M 



(6.54) 
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By applying the inequality in Equation 6.53 in Equation 6.54, we have 



V a (y,u)<-W(y)-kF(tm + F(t) 



v(0 , K (y,t)v(t) 
M M 



(6.55) 



< -W(y) - kFmt ) + F(t)\ 1 + y° l v(f) 
L M 



Let us choose the control law v(f) as 




(6.56) 



where w T (t) = 1{ 'm and q(y,f) > y (y,t). Using this control law, Equation 6.55 
becomes 



Hence, we have asymptotic stability with control law (Equation 6.56), As long 
as k > 0. Thus, the robust feedback control is given as u(t) = u(t)+ v(f), with 
u{t) given by Equation 6.47 and v(t) given by Equation 6.56. 

6.2.3 Robust Control: Both Matched and Unmatched Uncertainties 

Now let us consider both matched and unmatched uncertainties for the dynamic 
model. This means that there is uncertainty in the input u(t) to the overall system 
(Equation 6.26) and also in the input u(t ) to the system in Equation 6.27. Therefore, 
we have our uncertain model as 



Here the uncertainties are both matched and unmatched. The errors 0j and 0 2 are 
given by Equations 6.52 and 6.36 as Q 1 (t,y,u)= \\{t) and Q 2 (t,y,u) = g(y)v 2 (t), re- 
spectively, and are bounded as ll0 1 (t,y,M)ll < K(y,f) llv' 1 (t)ll and II0-, (T, w)ll < y(y,t) + 
K(y,r)llv 2 (t)ll. The control design scheme will be a combination of the designs for 
matched and unmatched uncertainties obtained in Sections 6.2.1 and 6.2.2. We pro- 
ceed with the control design as follows: 

1. Control by Lyapunov redesign: The first step is to design a robust feed- 
back controller u(t) = H(y,t)+ v 2 (t) for Equation 6.58 using the Lyapunov 
redesign technique. The control is given by Equation 6.39 as 



V a (y,u) < -W(y)—kF(t)l(t) 



Mu = u(t)+ 0j(f) 



(6.57) 



y= g(y)[u(t)+Q 2 (t)] 



(6.58) 



u(t) - H(y,t ) ~ , w(y,t) 

1-K (y,t) 



(6.59) 
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with v 2 (f) — — l-Khl.l) w(y,t) and H(y,t ) given by Equation 6.6. The controller 
stabilizes Equation 6.58 in the presence of uncertainty 0 2 . The Lyapunov 
function is given by Equation 6.33 as V(y,t) = jy T (t)y(t). 

2. Control by backstepping: The next step is to design a controller for the nom- 
inal model (model without uncertainty 0,) using the backstepping approach. 
The model given by Equations 6.28 and 6.29 is the nominal model: 



Mil = u 


(6.60) 


y = g(y)hi(t)+Q 2 (t)\ 


(6.61) 



The controller u ( t ) is given by Equation 6.43 as the following: 



u{t) = M[u n (t) + 



— g(y)u(t)] 

dy 



(6.62) 



The control law u n (t) is given by Equation 6.42 as 



u n (t') = -^ g(y)-kl(t) 
dy 



dV(y) 



g(y)-ki(t ) 



dy,- 

^yig(y)- ki(t) 



(6.63) 



i=l 

6 



with /(f) = u(t)~ H(y,t) and H(y,t) — H(y,t)— i^(y o vc(y,f). The modified 
Lyapunov function for this system is 



V a (y,u) = V(y)+^F(tm 

3. Control by Lyapunov redesign: In the final step we design the robust feed- 
back control u(t) = u(t)+ v,(f) for the overall system (Equation 6.57) using 
the Lyapunov redesign obtained in Section 6.2.2. The feedback control is 
thus given as 



u(t) = u(t)+ Vj(f) 



u(t ) 



r\(yd) 
1-k (y,t) 



w{t) 



(6.64) 



©2011 by Taylor and Francis Group, LLC 



142 



Autonomous Underwater Vehicles 



The control law u{t) is given by Equation 6.62. The Lyapunov function 
for this system is the same as in the previous step. Therefore, we achieve 
asymptotic stability and disturbance attenuation with this controller. 

In this chapter we designed robust feedback controllers for kinematic and dynamic 
models of underwater vehicles. We designed controllers for uncertainty in the input. 
For the kinematic model the uncertainty is matched and we used the method of 
Lyapunov redesign, which achieved the attenuation of disturbance. For the dynamic 
model we discussed both matched and unmatched uncertainties, and a combina- 
tion of both. We used the method of robust backstepping, which is a combination of 
Lyapunov redesign and backstepping. In all the controllers we achieved both asymp- 
totic stability and disturbance attenuation. 
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